Initial import of ORISYS arm workstation
This commit is contained in:
cheng
+17
@@ -0,0 +1,17 @@
|
||||
# Local ROS/colcon build products and runtime logs
|
||||
/build/
|
||||
/install/
|
||||
/log/
|
||||
|
||||
# Python caches and local virtual environments
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
.pytest_cache/
|
||||
.mypy_cache/
|
||||
.venv/
|
||||
|
||||
# Editor and desktop metadata
|
||||
.idea/
|
||||
.vscode/
|
||||
.DS_Store
|
||||
*~
|
||||
@@ -0,0 +1,66 @@
|
||||
# orisys_arm
|
||||
|
||||
能在这台机器上从源码编起来、再按三条命令启动的真机工程。项目目录仍叫 `orisys_arm`，应用包名是 `arm_application`。
|
||||
|
||||
## 源码
|
||||
|
||||
| 目录 | 来源 |
|
||||
|---|---|
|
||||
| `src/arm_application` | 相机、跟踪、伺服、对孔、旋入、夹爪、触觉 |
|
||||
| `src/arm_bringup` | `run_real.sh` 用的启动 |
|
||||
| `src/arm_control` | 状态、命名位姿、轨迹适配 |
|
||||
| `src/arm_driver_flexiv` | RDK 桥 |
|
||||
| `src/arm_msgs` | 孔检测、触觉的消息 |
|
||||
| `src/arm_description` | 本栈 URDF 包装 |
|
||||
| `src/arm_kinematics` | 几何逆解 |
|
||||
| `src/arm_interfaces` | 驱动读站点配置 |
|
||||
| `src/flexiv_description` | 官方机器人模型（拷贝，不是链接） |
|
||||
| `src/flexiv_ros2` | 官方 ROS 2 包（拷贝）。已去掉 `COLCON_IGNORE` |
|
||||
|
||||
现场配置在 `config/site.yaml`，手眼在 `handeye/`，权重在 `models/yolo/`，RDK 源码在 `thirdparty/flexiv_rdk`。运行时仍使用本机已装好的 `~/flexiv_install` 和 conda 环境 `ros_flexiv`。
|
||||
|
||||
## 启动
|
||||
|
||||
先在这个目录编译（不要在中文路径下编；本目录是 ASCII）：
|
||||
|
||||
```bash
|
||||
cd ~/桌面/orisys_arm
|
||||
bash scripts/build_all.sh
|
||||
```
|
||||
|
||||
三个终端：
|
||||
|
||||
```bash
|
||||
bash ~/桌面/orisys_arm/scripts/run_real.sh --arm 89
|
||||
bash ~/桌面/orisys_arm/scripts/run_camera_d405.sh --arm 89 --preview
|
||||
bash ~/桌面/orisys_arm/scripts/run_all.sh --arm 89
|
||||
```
|
||||
|
||||
手眼示教和标定在 `handeye/run_handeye.sh`。工位窗口在 `station/run_station.sh`。两者都不在 `scripts/`。没有搬仿真、MoveIt 和通止规装配状态机。
|
||||
|
||||
|
||||
## Qt 工位窗口
|
||||
|
||||
```bash
|
||||
bash station/run_station.sh
|
||||
```
|
||||
|
||||
启动器使用 `ros_flexiv` 的 Python，把 PySide6 / shiboken6 6.8.3 独立安装在
|
||||
`~/.local/share/orisys_arm/qt-6.8.3-py311`（设置了 `XDG_DATA_HOME` 时跟随该目录）。
|
||||
首次启动需要下载依赖，以后直接使用本地安装，不修改 conda 中的 ROS 依赖。
|
||||
`desk/run_desk.sh` 也使用同一套 Qt 运行时。
|
||||
当前搬迁的安装目录包含指向 `/tmp/orisys_mnt` 的链接，工位启动器会在私有挂载空间
|
||||
恢复该路径，避免 ROS 消息和相机桥因安装链接失效而加载失败。
|
||||
|
||||
本机原有 PySide6 6.12.0 在 Python 3.11 下执行 Qt 无返回值调用时会持续递减
|
||||
`None` 引用计数，造成约一分钟后异常退出或卡死。已移除手工补引用计数的补丁，
|
||||
并固定使用回归测试通过的版本。界面状态读取缓存站点配置，文件变化后自动重读，
|
||||
避免每次刷新重复解析同一份 YAML。
|
||||
|
||||
界面依赖回归测试（不连接设备、不下发动作）：
|
||||
|
||||
```bash
|
||||
bash station/run_station.sh --self-test
|
||||
```
|
||||
|
||||
工位已拆分为 Qt 前端、独立命令后端和常驻 ROS 驱动。设计、设备归属、迁移边界及测试见 [station/README.md](station/README.md)。
|
||||
@@ -0,0 +1,9 @@
|
||||
# config/
|
||||
|
||||
| 文件 | git | 说明 |
|
||||
|------|-----|------|
|
||||
| `site.example.yaml` | 提交 | 模板。克隆后复制为 `site.yaml` |
|
||||
| `site.yaml` | 不提交 | 本机 SN、IP、相机、锁高、权重路径 |
|
||||
| `fastdds_visual_servo.xml` | 提交 | 可选 DDS；默认 `site.yaml` 里 `fastdds_profile: ""` |
|
||||
|
||||
改跟踪 / 手眼 / 相机：**只改 `site.yaml`**。字段说明见 [docs/SITE_CONFIG.md](../docs/SITE_CONFIG.md)。
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 354 KiB |
@@ -0,0 +1,84 @@
|
||||
<?xml version="1.0" encoding="UTF-8" ?>
|
||||
<!--
|
||||
visual-servo 硬实时路径用的 FastDDS 配置（Ubuntu 22.04 / Humble 默认 RMW）。
|
||||
|
||||
用法：
|
||||
export FASTRTPS_DEFAULT_PROFILES_FILE=~/ros2/config/fastdds_visual_servo.xml
|
||||
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
|
||||
|
||||
目标：
|
||||
- 低延迟的控制话题优先使用 UDP 单播
|
||||
- 缩短发现 / 参与者租约，避免后加入的节点把 100 Hz 卡住
|
||||
- 保持较浅的历史队列，让订阅者永远不必去追积压的旧数据
|
||||
-->
|
||||
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
|
||||
<transport_descriptors>
|
||||
<transport_descriptor>
|
||||
<transport_id>udp_transport</transport_id>
|
||||
<type>UDPv4</type>
|
||||
<!-- 避免繁重的感知与伺服之间因共享内存积压而卡顿 -->
|
||||
<maxMessageSize>65500</maxMessageSize>
|
||||
</transport_descriptor>
|
||||
</transport_descriptors>
|
||||
|
||||
<participant profile_name="visual_servo_participant" is_default_profile="true">
|
||||
<rtps>
|
||||
<userTransports>
|
||||
<transport_id>udp_transport</transport_id>
|
||||
</userTransports>
|
||||
<useBuiltinTransports>false</useBuiltinTransports>
|
||||
<builtin>
|
||||
<discovery_config>
|
||||
<!-- 加快发现；缩短租约，让已经死掉的节点尽快被剔除 -->
|
||||
<leaseDuration>
|
||||
<sec>10</sec>
|
||||
<nanosec>0</nanosec>
|
||||
</leaseDuration>
|
||||
<leaseAnnouncement>
|
||||
<sec>3</sec>
|
||||
<nanosec>0</nanosec>
|
||||
</leaseAnnouncement>
|
||||
<initialAnnouncements>
|
||||
<count>5</count>
|
||||
<period>
|
||||
<sec>0</sec>
|
||||
<nanosec>100000000</nanosec>
|
||||
</period>
|
||||
</initialAnnouncements>
|
||||
</discovery_config>
|
||||
</builtin>
|
||||
</rtps>
|
||||
</participant>
|
||||
|
||||
<!-- 感知图像话题：BEST_EFFORT + KEEP_LAST（丢弃旧帧） -->
|
||||
<data_reader profile_name="sensor_best_effort">
|
||||
<qos>
|
||||
<reliability>
|
||||
<kind>BEST_EFFORT</kind>
|
||||
</reliability>
|
||||
<history>
|
||||
<kind>KEEP_LAST</kind>
|
||||
<depth>5</depth>
|
||||
</history>
|
||||
<durability>
|
||||
<kind>VOLATILE</kind>
|
||||
</durability>
|
||||
</qos>
|
||||
</data_reader>
|
||||
|
||||
<!-- /visual_servo/cmd_vel：永远只保留最新的 Twist -->
|
||||
<data_writer profile_name="cmd_vel_latest">
|
||||
<qos>
|
||||
<reliability>
|
||||
<kind>RELIABLE</kind>
|
||||
</reliability>
|
||||
<history>
|
||||
<kind>KEEP_LAST</kind>
|
||||
<depth>1</depth>
|
||||
</history>
|
||||
<durability>
|
||||
<kind>VOLATILE</kind>
|
||||
</durability>
|
||||
</qos>
|
||||
</data_writer>
|
||||
</profiles>
|
||||
@@ -0,0 +1,120 @@
|
||||
# 左臂 89 当前生效的参数。从旧仓库 config/site.yaml 抽出，去掉注释。
|
||||
# 节点代码还在读旧的 site.yaml，这一份还没接上。
|
||||
|
||||
robot:
|
||||
sn: "Rizon4-063989"
|
||||
ip: "192.168.2.101"
|
||||
pc_ip: "192.168.2.50"
|
||||
iface: "enp4s0"
|
||||
tool_name: "tool10120938"
|
||||
tcp_xyz_m: [-0.0005, 0.0067, 0.2245]
|
||||
tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
|
||||
workpiece_surface_z_m: 0.022
|
||||
handeye_file: "handeye/results/89_eye_in_hand.yaml"
|
||||
ft_wrench_topic: "/Rizon4_063989/external_wrench_in_tcp"
|
||||
|
||||
camera:
|
||||
wrist_name: "d405_left"
|
||||
wrist_serial: "352122274408"
|
||||
wrist_ae_limit_us: 4000
|
||||
wrist_emitter: false
|
||||
overhead_width: 848
|
||||
overhead_height: 480
|
||||
overhead_fps: 15
|
||||
overhead_ae_limit_us: 8000
|
||||
|
||||
yolo:
|
||||
weights: "models/yolo/gongjian0929.onnx"
|
||||
backend: "onnx"
|
||||
conf: 0.10
|
||||
iou: 0.45
|
||||
imgsz: 640
|
||||
device: "0"
|
||||
half_precision: true
|
||||
min_conf: 0.12
|
||||
reacquire_conf: 0.55
|
||||
vision_hz: 30.0
|
||||
feature_mode: "keypoint"
|
||||
keypoint_index: 0
|
||||
keypoint_min_conf: 0.30
|
||||
keypoint_border_px: 6
|
||||
track_max_step_m: 0.04
|
||||
reacquire_far_px: 120
|
||||
reacquire_confirm_frames: 3
|
||||
jump_px: 400.0
|
||||
jump_z_m: 0.10
|
||||
z_ref_m: 0.22
|
||||
min_depth_m: 0.04
|
||||
max_depth_m: 0.55
|
||||
feature_min_range_m: 0.14
|
||||
meas_blend: 0.85
|
||||
camera_frame: "d405_color_optical_frame"
|
||||
|
||||
servo:
|
||||
control_hz: 100.0
|
||||
ee_z_m: 0.320
|
||||
hole_hover_z_m: 0.320
|
||||
min_ee_z_m: 0.20
|
||||
safe_z_m: 0.45
|
||||
z_min_m: 0.22
|
||||
xy_track: "hover"
|
||||
ee_vertical_down: true
|
||||
kp_xy: 4.20
|
||||
kd_xy: 0.20
|
||||
kp_z: 0.80
|
||||
kd_z: 0.35
|
||||
deadzone_m: 0.003
|
||||
v_max_xy: 0.45
|
||||
v_max_z: 0.04
|
||||
slow_far_m: 0.05
|
||||
v_slow_xy: 0.08
|
||||
a_max: 3.00
|
||||
workspace_xyz_min: [0.08, -0.55, 0.18]
|
||||
workspace_xyz_max: [0.85, 0.55, 0.65]
|
||||
wrist_align_sec: 0.5
|
||||
d435_handback_sec: 0.5
|
||||
jump_confirm_frames: 3
|
||||
traj_topic: "rizon_arm_controller/joint_trajectory"
|
||||
point_topic: "visual_servo/filtered_point"
|
||||
|
||||
hole:
|
||||
backend: "two_stage"
|
||||
conf: 0.25
|
||||
iou: 0.7
|
||||
imgsz: 640
|
||||
target_hole: "k1"
|
||||
align_tol_m: 0.00025
|
||||
align_max_iters: 12
|
||||
final_gap_mm: 2.0
|
||||
observe_gap_mm: 15.0
|
||||
hough_param2: 26
|
||||
hough_min_radius: 6
|
||||
hough_max_radius: 30
|
||||
expected_holes: 2
|
||||
|
||||
perception:
|
||||
source: "yolo"
|
||||
track_camera: "wrist"
|
||||
wrist_handoff_conf: 0.5
|
||||
overhead_min_depth_m: 0.25
|
||||
overhead_max_depth_m: 1.80
|
||||
overhead_oneshot_conf: 0.5
|
||||
overhead_cooldown_s: 1.5
|
||||
|
||||
gripper:
|
||||
port: "/dev/ttyUSB0"
|
||||
baud: 115200
|
||||
slave_id: 1
|
||||
stroke_mm: 20.0
|
||||
force_pct: 20
|
||||
speed_pct: 50
|
||||
auto_init: false
|
||||
|
||||
tactile:
|
||||
model: "dd03"
|
||||
left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.1:1.0-video-index0"
|
||||
right_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.2:1.0-video-index0"
|
||||
baseline_seconds: 2.0
|
||||
filter_alpha: 0.25
|
||||
target_fps: 30.0
|
||||
publish_hz: 20.0
|
||||
File diff suppressed because it is too large
Load Diff
+1146
File diff suppressed because it is too large
Load Diff
@@ -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]
|
||||
@@ -0,0 +1,63 @@
|
||||
# YOLO 工件检测权重
|
||||
|
||||
> **改这个目录之前先读 [`../README.md`](../README.md)**（权重模块的规则）。
|
||||
|
||||
## 放什么
|
||||
|
||||
同名的 `.pt` / `.onnx` / `.engine` / `.labels.yaml` 放一起，名字主干必须一致：
|
||||
|
||||
```
|
||||
detect_best_0.onnx ← 写进 config/site.yaml 的那个
|
||||
detect_best_0.engine ← TensorRT 引擎，backend: tensorrt 时自动使用
|
||||
detect_best_0.labels.yaml ← 类别标签（进 git，可手改）
|
||||
```
|
||||
|
||||
二进制不进 git；`.labels.yaml` 进 git。
|
||||
|
||||
## 加新模型
|
||||
|
||||
```bash
|
||||
cp ~/下载/xxx.pt ~/ros2/models/staging/
|
||||
bash ~/ros2/scripts/weights.sh convert staging/xxx.pt --name workpiece --group yolo
|
||||
```
|
||||
|
||||
然后改 `config/site.yaml`：
|
||||
|
||||
```yaml
|
||||
paths:
|
||||
yolo_weights: "/home/USER/ros2/models/yolo/workpiece.onnx"
|
||||
yolo:
|
||||
backend: "tensorrt"
|
||||
imgsz: 640 # 必须等于该模型输入尺寸
|
||||
```
|
||||
|
||||
## 查看 / 修改标签
|
||||
|
||||
```bash
|
||||
bash ~/ros2/scripts/weights.sh labels detect_best_0
|
||||
bash ~/ros2/scripts/weights.sh labels detect_best_0 --set "工件,通止规"
|
||||
```
|
||||
|
||||
## 工作区里有哪些模型
|
||||
|
||||
只保留**真正在用**的两个，避免 `weights.sh list` 被无用模型干扰、也避免误选到废模型：
|
||||
|
||||
| 模型 | 用途 |
|
||||
|---|---|
|
||||
| `yolo/detect_best_0.onnx` + `.engine` | 工件/跟踪模型，即 `config/site.yaml → paths.yolo_weights` |
|
||||
| `hole_insert/detect_best_1.onnx` | 孔/工件检测，即 `hole_detect.weights` 与 `workpiece_weights` |
|
||||
|
||||
曾经在这里、现已移出仓库的模型（`detect_hc90`、`detect_best`、`detect_yolo26_gongjian`）
|
||||
是零引用的历史遗留，二进制权重不进 git，所以是**移出到备份目录**而非直接删除；
|
||||
需要时搬回本目录即可，`weights.sh list` 会自动重新发现。
|
||||
|
||||
## 排错
|
||||
|
||||
```bash
|
||||
bash ~/ros2/scripts/weights.sh doctor --check-engines
|
||||
```
|
||||
|
||||
`engine` 和 TensorRT 版本绑死：换过 TensorRT 后旧 engine 会加载失败，重导即可
|
||||
（`convert <同名>.onnx --name <名字> --to engine --force`）。
|
||||
**注意 `doctor` 能查出这类静默失效** —— engine 反序列化返回空时不会报错，
|
||||
只会安静地不退任何检测框，很容易被当成"模型效果差"。
|
||||
Binary file not shown.
@@ -0,0 +1,16 @@
|
||||
# 模型类别标签（class names）
|
||||
#
|
||||
# 一个模型对应一份标签，放在权重旁边、同名前缀：
|
||||
# detect_best_0.engine ← 权重
|
||||
# detect_best_0.labels.yaml ← 本文件
|
||||
#
|
||||
# 写法：names 下面一行一个类名，第 1 行对应类别 0，第 2 行对应类别 1，依次类推。
|
||||
# 改完保存即生效，不需要重新转换模型、也不用重启环境（重跑节点即可）。
|
||||
#
|
||||
# 命令行查看/修改（推荐，不用手改本文件）：
|
||||
# bash ~/ros2/scripts/weights.sh labels <模型名>
|
||||
# bash ~/ros2/scripts/weights.sh labels <模型名> --set "工件,通止规"
|
||||
|
||||
model: detect_best_0
|
||||
names:
|
||||
- workpiece
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,16 @@
|
||||
# 模型类别标签（class names）
|
||||
#
|
||||
# 一个模型对应一份标签，放在权重旁边、同名前缀：
|
||||
# detect_best_0.engine ← 权重
|
||||
# detect_best_0.labels.yaml ← 本文件
|
||||
#
|
||||
# 写法：names 下面一行一个类名，第 1 行对应类别 0，第 2 行对应类别 1，依次类推。
|
||||
# 改完保存即生效，不需要重新转换模型、也不用重启环境（重跑节点即可）。
|
||||
#
|
||||
# 命令行查看/修改（推荐，不用手改本文件）：
|
||||
# bash ~/ros2/scripts/weights.sh labels <模型名>
|
||||
# bash ~/ros2/scripts/weights.sh labels <模型名> --set "工件,通止规"
|
||||
|
||||
model: gongjian0929
|
||||
names:
|
||||
- workpiece
|
||||
Binary file not shown.
Executable
+42
@@ -0,0 +1,42 @@
|
||||
#!/usr/bin/env bash
|
||||
set -eo pipefail
|
||||
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
cd "$ROOT"
|
||||
|
||||
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
|
||||
elif [[ -f "${HOME}/miniconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/miniconda3/etc/profile.d/conda.sh"
|
||||
set +u
|
||||
conda activate ros_flexiv 2>/dev/null || true
|
||||
set -u 2>/dev/null || true
|
||||
fi
|
||||
|
||||
if [[ -z "${ROS_DISTRO:-}" ]]; then
|
||||
if [[ -f /opt/ros/jazzy/setup.bash ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
elif [[ -f /opt/ros/humble/setup.bash ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/humble/setup.bash
|
||||
elif [[ -f /opt/ros/iron/setup.bash ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/iron/setup.bash
|
||||
else
|
||||
echo "未检测到系统 ROS 2；使用 conda ros_flexiv 环境"
|
||||
fi
|
||||
fi
|
||||
|
||||
PACKAGES=(flexiv_description arm_msgs arm_description arm_kinematics arm_interfaces arm_control arm_driver_flexiv arm_bringup arm_application)
|
||||
# conda GCC 15 把 Humble 消息头里缺的 cstdint 当成错误。补上这个头，仍用 conda 编译器，
|
||||
# 才能和 ros_flexiv 里的 librclcpp 链在一起。
|
||||
export CXXFLAGS="${CXXFLAGS:-} -include cstdint -Wno-template-body"
|
||||
colcon build --symlink-install --packages-select "${PACKAGES[@]}" "$@"
|
||||
# shellcheck source=lib/fix_shebangs.sh
|
||||
source "${ROOT}/scripts/lib/fix_shebangs.sh"
|
||||
fix_shebangs "${ROOT}/install" "${CONDA_PREFIX:-/usr}/bin/python3"
|
||||
|
||||
echo "构建完成。请执行: source ${ROOT}/install/setup.bash"
|
||||
Executable
+33
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env bash
|
||||
# 构建整个工作区：机械臂栈 + Flexiv 真机驱动
|
||||
set -eo pipefail
|
||||
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
cd "$ROOT"
|
||||
|
||||
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
|
||||
elif [[ -f "${HOME}/miniconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/miniconda3/etc/profile.d/conda.sh"
|
||||
set +u
|
||||
conda activate ros_flexiv 2>/dev/null || true
|
||||
set -u 2>/dev/null || true
|
||||
fi
|
||||
|
||||
if [[ -z "${ROS_DISTRO:-}" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/humble/setup.bash 2>/dev/null || source /opt/ros/jazzy/setup.bash 2>/dev/null || true
|
||||
fi
|
||||
|
||||
echo "=== [1/2] arm stack ==="
|
||||
bash "${ROOT}/scripts/build.sh"
|
||||
|
||||
echo "=== [2/2] flexiv real stack ==="
|
||||
bash "${ROOT}/scripts/build_real.sh"
|
||||
|
||||
echo ""
|
||||
echo "Done. Use:"
|
||||
echo " source ${ROOT}/scripts/env.sh"
|
||||
echo " bash ${ROOT}/scripts/run_real.sh"
|
||||
Executable
+40
@@ -0,0 +1,40 @@
|
||||
#!/usr/bin/env bash
|
||||
# 构建真机用的 Flexiv 硬件包（前提：RDK 已装到 ~/flexiv_install）
|
||||
set -eo pipefail
|
||||
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
cd "$ROOT"
|
||||
|
||||
if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/anaconda3/etc/profile.d/conda.sh"
|
||||
elif [[ -f "${HOME}/miniconda3/etc/profile.d/conda.sh" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${HOME}/miniconda3/etc/profile.d/conda.sh"
|
||||
set +u
|
||||
conda activate ros_flexiv 2>/dev/null || true
|
||||
set -u 2>/dev/null || true
|
||||
fi
|
||||
|
||||
if [[ -z "${ROS_DISTRO:-}" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/humble/setup.bash 2>/dev/null || source /opt/ros/jazzy/setup.bash 2>/dev/null || true
|
||||
fi
|
||||
|
||||
PREFIX="${HOME}/flexiv_install"
|
||||
CMAKE_PREFIX="${PREFIX}"
|
||||
if [[ -n "${CONDA_PREFIX:-}" ]]; then
|
||||
CMAKE_PREFIX="${PREFIX};${CONDA_PREFIX}"
|
||||
fi
|
||||
|
||||
export CXXFLAGS="${CXXFLAGS:-} -include cstdint -Wno-template-body"
|
||||
colcon build --symlink-install --packages-select \
|
||||
flexiv_hardware \
|
||||
flexiv_msgs \
|
||||
flexiv_gripper \
|
||||
flexiv_test_nodes \
|
||||
flexiv_robot_states_broadcaster \
|
||||
flexiv_bringup \
|
||||
--cmake-args "-DCMAKE_PREFIX_PATH=${CMAKE_PREFIX}" "$@"
|
||||
|
||||
echo "Real stack build done. source ${ROOT}/scripts/env.sh"
|
||||
Executable
+64
@@ -0,0 +1,64 @@
|
||||
#!/usr/bin/env bash
|
||||
# 加载本工作区环境（conda + ROS + flexiv 库）。
|
||||
# 用法：source scripts/env.sh
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
|
||||
if [[ -f /opt/ros/humble/setup.bash ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/humble/setup.bash
|
||||
elif [[ -f /opt/ros/jazzy/setup.bash ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
fi
|
||||
|
||||
# conda 可能在 ~/anaconda3 或 ~/miniconda3。不写死用户名。
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/dev_tools.sh"
|
||||
dev_source_conda || true
|
||||
if command -v conda >/dev/null 2>&1; then
|
||||
# 干净切换：先把堆叠的激活退到 base，再激活真机环境。
|
||||
# 同一 shell 里从仿真环境切过来时，不清干净会残留 PATH/PYTHONPATH。
|
||||
_env_helper="${ROOT}/scripts/lib/conda_switch.sh"
|
||||
_real_env="${REAL_ENV_NAME:-ros_flexiv}"
|
||||
if [[ -f "${_env_helper}" ]]; then
|
||||
# shellcheck disable=SC1090
|
||||
source "${_env_helper}"
|
||||
conda_switch_clean "${_real_env}" "${SIM_ENV_NAME:-ros_gz_sim}" || \
|
||||
conda activate "${_real_env}" 2>/dev/null || true
|
||||
else
|
||||
conda activate "${_real_env}" 2>/dev/null || true
|
||||
fi
|
||||
unset _env_helper _real_env
|
||||
fi
|
||||
|
||||
if [[ -f "${ROOT}/install/setup.bash" ]]; then
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/install/setup.bash"
|
||||
# 本机 /opt/ros/humble 会盖住 conda 的 ros2，3.10 解释器加载不了 3.11 的 rclpy。
|
||||
if [[ -n "${CONDA_PREFIX:-}" && -x "${CONDA_PREFIX}/bin/ros2" ]]; then
|
||||
export PATH="${CONDA_PREFIX}/bin:${PATH}"
|
||||
fi
|
||||
else
|
||||
echo "警告：${ROOT}/install 还没编译。工位窗口可以先开；真机栈要先在仓库根目录 colcon build（不要在中文路径下编）。" >&2
|
||||
fi
|
||||
export LD_LIBRARY_PATH="${HOME}/flexiv_install/lib:${LD_LIBRARY_PATH:-}"
|
||||
# pip 版 TensorRT（tensorrt-cu12）—— 加载 YOLO `.engine` 需要它
|
||||
_pyver="$(python3 -c 'import sys; print(f"{sys.version_info[0]}.{sys.version_info[1]}")' 2>/dev/null || echo 3.11)"
|
||||
_TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/${REAL_ENV_NAME:-ros_flexiv}}/lib/python${_pyver}/site-packages"
|
||||
for _d in "${_TRT_SITE}/tensorrt" "${_TRT_SITE}/tensorrt_libs" "${_TRT_SITE}/nvidia/tensorrt/lib"; do
|
||||
if [[ -d "${_d}" ]]; then
|
||||
export LD_LIBRARY_PATH="${_d}:${LD_LIBRARY_PATH}"
|
||||
fi
|
||||
done
|
||||
unset _d _TRT_SITE _pyver
|
||||
export YOLO_AUTOINSTALL=false
|
||||
export ROS2_WS="${ROOT}"
|
||||
|
||||
# 真机与仿真的 DDS 隔离（细节与踩坑见 lib/ros_domain.sh 的文件头）。
|
||||
# shellcheck source=lib/ros_domain.sh
|
||||
source "${ROOT}/scripts/lib/ros_domain.sh"
|
||||
ros_domain_force_real
|
||||
|
||||
# 主站点配置 → `SITE_*`（YOLO 权重、相机、机器人 SN……）
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
|
||||
@@ -0,0 +1,103 @@
|
||||
#!/usr/bin/env bash
|
||||
# =============================================================================
|
||||
# conda 环境切换的公共逻辑。被 scripts/env.sh 与 scripts/sim_env.sh 共用。
|
||||
#
|
||||
# 为什么需要这个：conda 的 activate 是**堆叠**的。在同一个 shell 里反复
|
||||
# activate / 切换不同环境，会不断往 PATH、PYTHONPATH 里追加条目，而不会
|
||||
# 清掉前一个环境留下的。实测的后果非常隐蔽：
|
||||
#
|
||||
# 在同一个 shell 里先 activate ros_gz_sim 再切 ros_flexiv 之后，
|
||||
# `which python` 指向 ros_flexiv/bin/python，
|
||||
# 但 `python -m pip` 却解析到 ros_gz_sim 的 pip！
|
||||
# 于是 colcon --symlink-install 报：
|
||||
# error: option --editable not recognized
|
||||
# 这个报错和真实原因（shell 脏了）毫无关系，极易被误判成
|
||||
# "环境被搞坏了 / setuptools 版本不对"，进而去做一堆无用的修复。
|
||||
#
|
||||
# 对策：切换前先把堆叠的激活全部退掉（退到 base），再激活目标环境。
|
||||
# =============================================================================
|
||||
|
||||
# conda_deactivate_all: 退到 base（CONDA_SHLVL=1）。任何环境下调用都安全。
|
||||
conda_deactivate_all() {
|
||||
local guard=0
|
||||
# CONDA_SHLVL 是 conda 自己维护的激活层数；base 为 1。
|
||||
# 加 guard 是为了防止某些畸形状态下无限循环。
|
||||
while [[ "${CONDA_SHLVL:-1}" -gt 1 && "${guard}" -lt 20 ]]; do
|
||||
conda deactivate 2>/dev/null || break
|
||||
guard=$((guard + 1))
|
||||
done
|
||||
return 0
|
||||
}
|
||||
|
||||
# conda_switch_clean <env_name> [<other_env_name>]
|
||||
# 干净地激活 <env_name>；若当前已在 <other_env_name> 里，先退到 base 再切，
|
||||
# 并提示用户最好开新 shell。
|
||||
# 返回 0 = 成功；非 0 = 目标环境不存在。
|
||||
conda_switch_clean() {
|
||||
local target="$1"
|
||||
local other="${2:-}"
|
||||
|
||||
if [[ -n "${other}" && "${CONDA_DEFAULT_ENV:-}" == "${other}" ]]; then
|
||||
echo "提示: 当前 shell 激活的是 ${other}，即将切到 ${target}。" >&2
|
||||
echo " conda 的激活是堆叠的，同一 shell 里来回切会残留 PATH/PYTHONPATH，" >&2
|
||||
echo " 典型症状是构建报 \"--editable not recognized\" 这类与真实原因无关的错。" >&2
|
||||
echo " 这里会先退到 base 再切以规避；但最稳妥仍是开一个新 shell。" >&2
|
||||
fi
|
||||
|
||||
conda_deactivate_all
|
||||
|
||||
if ! conda activate "${target}" 2>/dev/null; then
|
||||
return 1
|
||||
fi
|
||||
return 0
|
||||
}
|
||||
|
||||
# path_drop_patterns <VAR> <glob>...
|
||||
# 从环境变量 <VAR> 的冒号分隔列表里删掉所有匹配任一 glob 的条目（保持剩余顺序）。
|
||||
# 用于清理"同一个 shell 里先 source 过另一套环境"留下的路径残留。
|
||||
#
|
||||
# 为什么必须清：真机环境（scripts/env.sh）与仿真环境（scripts/sim_env.sh）
|
||||
# 共用同一个工作区，各自的 setup.bash 都会往 PATH / PYTHONPATH /
|
||||
# LD_LIBRARY_PATH / AMENT_PREFIX_PATH 里追加自己的产物目录。conda 的
|
||||
# deactivate 只会撤掉 conda 自己那部分，**不会**撤掉 setup.bash 追加的条目。
|
||||
# 于是"先真机后仿真"的 shell 里会长成这样：
|
||||
# PYTHONPATH = .../install/arm_msgs/... : .../install_sim/arm_msgs/...
|
||||
# 此时 `import arm_msgs` 会拿到**真机那份**，与 install_sim 下的运行库混用，
|
||||
# 症状是消息反序列化莫名失败、字段对不上、dlopen 找不到 typesupport ——
|
||||
# 报错信息完全不提"环境脏了"这个真实原因。
|
||||
#
|
||||
# 实测踩过：sim_tactile_adapter 订阅 ros_gz_interfaces/msg/Contacts 时崩在
|
||||
# rcutils_set_error_state: Type support not from this implementation ...
|
||||
# Could not load library libros_gz_interfaces__rosidl_typesupport_fastrtps_c.so
|
||||
# 库其实就在 $CONDA_PREFIX/lib 下（ldd 全齐），根因是 LD_LIBRARY_PATH 里
|
||||
# 既残留着 ros_flexiv 的 lib、又**没有** $CONDA_PREFIX/lib。
|
||||
path_drop_patterns() {
|
||||
local _var="$1"
|
||||
shift
|
||||
local -a _pats=("$@")
|
||||
local _cur="${!_var:-}"
|
||||
[[ -z "${_cur}" ]] && return 0
|
||||
|
||||
local _out="" _entry _pat _hit
|
||||
local IFS=':'
|
||||
for _entry in ${_cur}; do
|
||||
_hit=0
|
||||
for _pat in "${_pats[@]}"; do
|
||||
# 故意不加引号：让右侧当 glob 用（这里就是要通配匹配）
|
||||
# shellcheck disable=SC2053
|
||||
if [[ "${_entry}" == ${_pat} ]]; then
|
||||
_hit=1
|
||||
break
|
||||
fi
|
||||
done
|
||||
[[ "${_hit}" -eq 1 ]] && continue
|
||||
_out="${_out:+${_out}:}${_entry}"
|
||||
done
|
||||
|
||||
printf -v "${_var}" '%s' "${_out}"
|
||||
export "${_var?}"
|
||||
return 0
|
||||
}
|
||||
|
||||
# 供调用方按需取消设置，避免污染使用者的变量空间。
|
||||
# （bash 没有"私有函数"概念，命名上统一加 conda_ 前缀降低撞名概率。）
|
||||
@@ -0,0 +1,31 @@
|
||||
# 本机工具链。仓库位置用调用方已经算好的 ROOT，不写死某个用户的家目录。
|
||||
# 用法：source "${ROOT}/scripts/lib/dev_tools.sh"
|
||||
|
||||
dev_source_conda() {
|
||||
local cand
|
||||
for cand in \
|
||||
"${HOME}/anaconda3/etc/profile.d/conda.sh" \
|
||||
"${HOME}/miniconda3/etc/profile.d/conda.sh"
|
||||
do
|
||||
if [[ -f "${cand}" ]]; then
|
||||
# shellcheck disable=SC1090
|
||||
source "${cand}"
|
||||
return 0
|
||||
fi
|
||||
done
|
||||
echo "ERROR: 找不到 conda.sh。试过 \$HOME/anaconda3 和 \$HOME/miniconda3。" >&2
|
||||
return 1
|
||||
}
|
||||
|
||||
dev_rdk_python() {
|
||||
if [[ -n "${RDK_PYTHON:-}" && -x "${RDK_PYTHON}" ]]; then
|
||||
printf '%s\n' "${RDK_PYTHON}"
|
||||
return 0
|
||||
fi
|
||||
if [[ -x "${HOME}/rdk_py310/bin/python" ]]; then
|
||||
printf '%s\n' "${HOME}/rdk_py310/bin/python"
|
||||
return 0
|
||||
fi
|
||||
echo "ERROR: 找不到 RDK 的 Python。设置 RDK_PYTHON，或装到 \$HOME/rdk_py310。" >&2
|
||||
return 1
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,82 @@
|
||||
#!/usr/bin/env bash
|
||||
# =============================================================================
|
||||
# 修 console_scripts 的 shebang（真机 / 仿真两套构建产物都会踩的坑）
|
||||
# =============================================================================
|
||||
#
|
||||
# 现象：
|
||||
# source scripts/sim_env.sh && ros2 run arm_sim_gazebo sim_camera_adapter
|
||||
# 进程列表里却是
|
||||
# /home/cc/anaconda3/envs/ros_flexiv/bin/python3.11 ... sim_camera_adapter
|
||||
# 即**仿真节点跑在真机的 conda 环境里**。
|
||||
#
|
||||
# 为什么：
|
||||
# setuptools 的 console_scripts 入口脚本，shebang 用的是**构建那一刻**的
|
||||
# sys.executable。本工作区只有一份源码，真机与仿真各有自己的 conda 环境
|
||||
# （ros_flexiv / ros_gz_sim）和各自的 install 目录。如果在真机环境里跑过
|
||||
# build_sim.sh（或反之），写进 install_sim/ 的脚本 shebang 就指向了另一个
|
||||
# 环境。`ros2 run` 是**照 shebang 执行**的，不看当前 CONDA_PREFIX。
|
||||
#
|
||||
# 为什么危险：
|
||||
# 两个环境都是 ROS 2 Humble + Python 3.11，所以多数时候"居然能跑"，
|
||||
# 只是 import 到的是另一个环境的包。表现为偶发的 ABI/`dlopen`/消息反序列化
|
||||
# 报错，报错信息完全不提"shebang 指错环境"。
|
||||
# 实测：LD_LIBRARY_PATH、PYTHONPATH 都清干净了，节点仍然跑在 ros_flexiv 里。
|
||||
#
|
||||
# 对策：
|
||||
# 构建收尾时扫一遍本次产物，把指向"别的 conda 环境"的 shebang 改回
|
||||
# **本环境**的 python。已指向本环境的文件不动（幂等）。
|
||||
#
|
||||
# 用法：
|
||||
# fix_shebangs <install_dir> <expected_python_path>
|
||||
# =============================================================================
|
||||
|
||||
# fix_shebangs <install_dir> <expected_python>
|
||||
# 把 <install_dir> 下所有 console_scripts 里"指向其它 conda 环境"的 shebang
|
||||
# 改写成 <expected_python>。返回 0；打印改了哪些文件（没改就不打印）。
|
||||
fix_shebangs() {
|
||||
local install_dir="$1"
|
||||
local expected="$2"
|
||||
|
||||
if [[ ! -d "${install_dir}" || ! -x "${expected}" ]]; then
|
||||
return 0
|
||||
fi
|
||||
|
||||
# 只扫这两类位置：包自带的 console_scripts 目录、以及 bin/。
|
||||
# 用 find 而不是递归 rg：要按"是不是可执行文件 + 首行是 shebang"判断。
|
||||
local -a files=()
|
||||
local f
|
||||
while IFS= read -r -d '' f; do
|
||||
files+=("${f}")
|
||||
done < <(find "${install_dir}" \
|
||||
\( -path "*/lib/*" -o -path "*/bin/*" \) \
|
||||
-type f -perm -u+x -print0 2>/dev/null)
|
||||
|
||||
local fixed=0 first
|
||||
for f in "${files[@]}"; do
|
||||
# 读首行（最多 200 字节）；不是 #! 开头就跳过（二进制、数据文件等）
|
||||
first="$(head -c 200 "${f}" | head -1)"
|
||||
[[ "${first:0:2}" == "#!" ]] || continue
|
||||
# 只动"指向某个 conda 环境"的 shebang；/usr/bin/python3 这类系统解释器不动
|
||||
[[ "${first}" == *"/envs/"*"python"* ]] || continue
|
||||
[[ "${first}" == *"${expected}"* ]] && continue
|
||||
|
||||
# 保留原 shebang 里的参数（例如 `python3 -u`）
|
||||
local interp="${first#\#!}" # 去掉 #!
|
||||
interp="${interp%% *}" # 第一个 token = 解释器路径
|
||||
local rest="${first#"#!${interp}"}" # 剩下的是参数（含前导空格）
|
||||
# 就地重写首行：用 printf + tail 组装，避免 sed -i 在符号链接上的坑
|
||||
local tmp="${f}.shebang.$$"
|
||||
{
|
||||
printf '#!%s%s\n' "${expected}" "${rest}"
|
||||
tail -n +2 "${f}"
|
||||
} > "${tmp}" && cat "${tmp}" > "${f}" && rm -f "${tmp}"
|
||||
echo " 修正 shebang: ${f#$PWD/} → ${expected}"
|
||||
fixed=$((fixed + 1))
|
||||
done
|
||||
|
||||
if [[ "${fixed}" -gt 0 ]]; then
|
||||
echo " （共修正 ${fixed} 个入口脚本：它们的 shebang 原本指向别的 conda 环境，"
|
||||
echo " 会导致 ros2 run 起来的节点跑在错误的环境里）"
|
||||
fi
|
||||
return 0
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
"""伺服和 flexiv_hardware 之间的 MoveL 交接。
|
||||
|
||||
两边不在一个进程里，硬件接口也没有自己的 ROS 节点。用三个文件：
|
||||
|
||||
* ``/tmp/flexiv_movel_req`` 一行 ``seq x y z keep_z vel``
|
||||
* ``/tmp/flexiv_movel_ack`` 硬件接住请求后立刻写序号
|
||||
* ``/tmp/flexiv_movel_done`` 一行 ``seq ok`` 或 ``seq fail 原因``
|
||||
|
||||
先写临时文件再改名，避免对面读到半行。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
REQ = Path("/tmp/flexiv_movel_req")
|
||||
ACK = Path("/tmp/flexiv_movel_ack")
|
||||
DONE = Path("/tmp/flexiv_movel_done")
|
||||
|
||||
|
||||
def format_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> str:
|
||||
return (
|
||||
f"{int(seq)} {float(x):.6f} {float(y):.6f} {float(z):.6f} "
|
||||
f"{1 if keep_z else 0} {float(vel):.4f}\n"
|
||||
)
|
||||
|
||||
|
||||
def write_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> None:
|
||||
text = format_request(seq, x, y, z, keep_z, vel)
|
||||
tmp = REQ.with_name(REQ.name + ".tmp")
|
||||
tmp.write_text(text, encoding="utf-8")
|
||||
tmp.replace(REQ)
|
||||
|
||||
|
||||
def parse_done(text: str):
|
||||
"""返回 ``(seq, ok, detail)``。行不完整时返回 None。"""
|
||||
parts = text.strip().split(maxsplit=2)
|
||||
if len(parts) < 2:
|
||||
return None
|
||||
try:
|
||||
seq = int(parts[0])
|
||||
except ValueError:
|
||||
return None
|
||||
if parts[1] not in ("ok", "fail"):
|
||||
return None
|
||||
detail = parts[2] if len(parts) > 2 else ""
|
||||
return seq, parts[1] == "ok", detail
|
||||
|
||||
|
||||
def read_done():
|
||||
try:
|
||||
text = DONE.read_text(encoding="utf-8")
|
||||
except OSError:
|
||||
return None
|
||||
return parse_done(text)
|
||||
|
||||
|
||||
def read_ack() -> int:
|
||||
try:
|
||||
text = ACK.read_text(encoding="utf-8").strip()
|
||||
except OSError:
|
||||
return -1
|
||||
try:
|
||||
return int(text.split()[0])
|
||||
except (ValueError, IndexError):
|
||||
return -1
|
||||
Executable
+50
@@ -0,0 +1,50 @@
|
||||
#!/usr/bin/env bash
|
||||
# 把 ~/ros2/config/site.yaml 加载成 `SITE_*` 环境变量。
|
||||
# 用法（在其它脚本里）：
|
||||
# source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
#
|
||||
# 本文件同时负责**在配置有问题时立刻拦下**，因为它的调用方几乎都是
|
||||
# `eval "$(...)"` 或直接引用 `SITE_*`：如果这里不拦，脚本会拿着一堆空变量
|
||||
# 继续往下跑，真正报错的地方会离原因很远（历史上就这么出过"参数静默失效"）。
|
||||
#
|
||||
# 两种"没配置好"要区别对待：
|
||||
# 1. site.yaml **不存在** —— 新克隆还没 `cp`。给 WARN 就放行，
|
||||
# 因为有些脚本（网络检查、RDK 自检）在配置前就该能用。
|
||||
# 2. site.yaml **存在但选不出机械臂** —— 配置写错了。直接拦下，
|
||||
# 并且必须给出"改哪个文件的哪个键"。
|
||||
|
||||
_SITE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)"
|
||||
_SITE_EXPORTER="${_SITE_ROOT}/scripts/lib/export_site_env.py"
|
||||
_SITE_FILE="${SITE_CONFIG:-${_SITE_ROOT}/config/site.yaml}"
|
||||
|
||||
# 供调用方判断"是没配置还是配错了"。0 = 正常加载；1 = 文件不存在；2 = 配置有误。
|
||||
SITE_LOAD_STATUS=0
|
||||
|
||||
if [[ ! -f "${_SITE_FILE}" ]]; then
|
||||
echo "[site] WARN: 没有 ${_SITE_FILE}（新克隆请先 cp config/site.example.yaml config/site.yaml）" >&2
|
||||
SITE_LOAD_STATUS=1
|
||||
else
|
||||
_site_env=""
|
||||
if ! _site_env="$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")"; then
|
||||
echo "[site] ERROR: 解析 ${_SITE_FILE} 失败，见上面 export_site_env.py 的报错。" >&2
|
||||
echo "[site] 修好后重试；不要带着空的 SITE_* 继续跑。" >&2
|
||||
SITE_LOAD_STATUS=2
|
||||
else
|
||||
# shellcheck disable=SC1090
|
||||
eval "${_site_env}"
|
||||
export SITE_CONFIG="${_SITE_FILE}"
|
||||
|
||||
if [[ -z "${SITE_ROBOT_SN:-}" ]]; then
|
||||
# 绝不兜底成某个 SN：那等于"配置漏了照样启动，连的是哪台看运气"。
|
||||
echo "[site] ERROR: ${_SITE_FILE} 里没有选出机械臂（robot.sn 为空且没有 --arm）。" >&2
|
||||
echo "[site] 在 config/site.yaml 的 robot.sn 里填完整 SN，" >&2
|
||||
echo "[site] 或给命令加 --arm <臂号>（臂号 = 该臂 SN 的末两位）。" >&2
|
||||
SITE_LOAD_STATUS=2
|
||||
else
|
||||
# 把 89/91/left/right 别名解析成完整的 Flexiv SN，供 RDK 使用。
|
||||
export ROBOT_SN="${SITE_ROBOT_SN}"
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
unset _SITE_ROOT _SITE_EXPORTER _SITE_FILE _site_env
|
||||
@@ -0,0 +1,26 @@
|
||||
#!/usr/bin/env bash
|
||||
# 从 argv 里解析 --sn / --arm。必须在 `load_site_config.sh` 之前 source，
|
||||
# 这样 `ROBOT_SN` 才能被 export_site_env.py 看到。
|
||||
#
|
||||
# source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||||
# parse_arm_args "$@"
|
||||
# set -- "${PARSE_ARM_REST[@]}"
|
||||
#
|
||||
# 别名（89|91|left|right）在 export_site_env.py 里解析。
|
||||
|
||||
parse_arm_args() {
|
||||
PARSE_ARM_REST=()
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--sn|--arm)
|
||||
export ROBOT_SN="${2:?}"
|
||||
export ROBOT_ARM="${2}"
|
||||
shift 2
|
||||
;;
|
||||
*)
|
||||
PARSE_ARM_REST+=("$1")
|
||||
shift
|
||||
;;
|
||||
esac
|
||||
done
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
#!/usr/bin/env bash
|
||||
# 由两个 Qt 启动器 source；独立目录固定 PySide6 和 shiboken，不替换 ROS 环境的包。
|
||||
orisys_qt_env() {
|
||||
local qt_version="6.8.3" qt_python qt_dir qt_temp
|
||||
qt_python="$(python3 -c 'import sys; print(f"{sys.version_info.major}{sys.version_info.minor}")')"
|
||||
qt_dir="${XDG_DATA_HOME:-${HOME}/.local/share}/orisys_arm/qt-${qt_version}-py${qt_python}"
|
||||
if ! PYTHONPATH="${qt_dir}" python3 -c 'import PySide6, shiboken6; assert PySide6.__version__ == shiboken6.__version__ == "6.8.3"' >/dev/null 2>&1; then
|
||||
echo "[Qt] 首次安装工位独立界面依赖 PySide6 ${qt_version}……" >&2
|
||||
mkdir -p "$(dirname "${qt_dir}")"
|
||||
qt_temp="$(mktemp -d "${qt_dir}.XXXXXX")"
|
||||
if ! python3 -m pip install --disable-pip-version-check --target "${qt_temp}" -r "${ROOT}/station/requirements-qt.txt"; then
|
||||
rm -rf "${qt_temp}"
|
||||
echo "[Qt] 界面依赖安装失败，请检查网络后重新启动。" >&2
|
||||
return 1
|
||||
fi
|
||||
rm -rf "${qt_dir}"
|
||||
mv "${qt_temp}" "${qt_dir}"
|
||||
fi
|
||||
export PYTHONPATH="${qt_dir}${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
# 避免继承 OpenCV 等组件设置的其他 Qt 版本插件目录。
|
||||
unset QT_PLUGIN_PATH QT_QPA_PLATFORM_PLUGIN_PATH
|
||||
}
|
||||
@@ -0,0 +1,261 @@
|
||||
#!/usr/bin/env python3
|
||||
"""RDK 操作公共函数（必须在 rdk_py310 / Python 3.10 下运行）。
|
||||
|
||||
背景与约束
|
||||
----------
|
||||
非夕的 `flexivrdk` Python 绑定与 ROS 环境的 Python 3.11 ABI 不兼容，所以所有
|
||||
直接调用 RDK 的代码必须以子进程方式跑在 `/home/cc/rdk_py310/bin/python` 下。
|
||||
因此本文件不能用任何 3.11+ 语法，也不 import ROS。
|
||||
|
||||
抽取原因
|
||||
--------
|
||||
`ensure_operational` / `ensure_idle` / tool 相关函数原先在
|
||||
`scripts/rdk_switch_tool.py` 与 `scripts/rdk_drag_bridge.py` 里各写了一份，
|
||||
行为已经出现细微差异（一个用 ServoOn 一个用 Enable，超时时间不同）。
|
||||
统一到本文件后，两处调用方行为一致。
|
||||
|
||||
重要限制
|
||||
--------
|
||||
RDK 对同一台机器人是**独占访问**：同时有两个进程连着同一个 SN 会失败或行为异常。
|
||||
所以框架里所有 RDK 操作都应当经由 `rdk_drag_bridge.py` 这一个长驻进程串行化，
|
||||
不要让业务节点各自起 RDK 连接。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import time
|
||||
from typing import Any, Dict, List, Optional
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 使能与故障
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def ensure_operational(robot: Any, timeout_s: float = 10.0) -> Optional[str]:
|
||||
"""确保机器人处于 operational（已使能、无故障）。
|
||||
|
||||
返回 ``None`` 表示成功，否则返回给用户看的错误说明。
|
||||
|
||||
这是幂等的：已经 operational 时不做任何事。
|
||||
"""
|
||||
try:
|
||||
if robot.fault():
|
||||
robot.ClearFault()
|
||||
time.sleep(0.5)
|
||||
if not robot.operational():
|
||||
# 不同 RDK 版本一个叫 ServoOn 一个叫 Enable。
|
||||
for fn in ("ServoOn", "Enable"):
|
||||
if hasattr(robot, fn):
|
||||
try:
|
||||
getattr(robot, fn)()
|
||||
break
|
||||
except Exception:
|
||||
pass
|
||||
deadline = time.monotonic() + timeout_s
|
||||
while not robot.operational() and time.monotonic() < deadline:
|
||||
time.sleep(0.2)
|
||||
if not robot.operational():
|
||||
return (
|
||||
"机器人未处于 operational 状态（故障 / 急停未复位？）。"
|
||||
"请在示教器上清故障、松开急停，并确认处于 AUTO(Remote)。"
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
return str(exc)
|
||||
return None
|
||||
|
||||
|
||||
def clear_fault(robot: Any) -> Optional[str]:
|
||||
"""清除故障。返回 ``None`` 表示成功。"""
|
||||
try:
|
||||
robot.ClearFault()
|
||||
time.sleep(0.5)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
return str(exc)
|
||||
return None
|
||||
|
||||
|
||||
def ensure_idle(robot: Any, mode: Any, timeout_s: float = 10.0) -> Optional[str]:
|
||||
"""确保机器人切到 IDLE 模式（工具切换等操作的前提）。
|
||||
|
||||
返回 ``None`` 表示成功，否则返回错误说明。
|
||||
"""
|
||||
try:
|
||||
robot.SwitchMode(mode.IDLE)
|
||||
return None
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
err = ensure_operational(robot, timeout_s=timeout_s)
|
||||
if err:
|
||||
return err
|
||||
try:
|
||||
robot.SwitchMode(mode.IDLE)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
return f"无法切到 IDLE: {exc}"
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 工具坐标系
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def single_arm_groups(robot: Any) -> List[Any]:
|
||||
"""返回工具 API 的关节组列表。
|
||||
|
||||
RDK 1.9 的 Tool API 是单臂的（没有 JointGroup 概念），此时返回 ``[None]``
|
||||
作为哨兵，调用方据此走不带 group 参数的分支。老版本返回实际组名列表。
|
||||
"""
|
||||
try:
|
||||
info = robot.info()
|
||||
groups = getattr(info, "single_arm_groups", None) or {}
|
||||
keys = list(groups.keys()) if isinstance(groups, dict) else list(groups)
|
||||
if keys:
|
||||
return keys
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
import flexivrdk
|
||||
|
||||
if hasattr(flexivrdk, "JointGroup"):
|
||||
return [flexivrdk.JointGroup.ARM_1]
|
||||
except Exception:
|
||||
pass
|
||||
return [None]
|
||||
|
||||
|
||||
def tool_name(tool: Any, group: Any) -> str:
|
||||
"""读当前工具名。"""
|
||||
try:
|
||||
return str(tool.name() if group is None else tool.name(group))
|
||||
except Exception:
|
||||
return ""
|
||||
|
||||
|
||||
def tool_params(tool: Any, group: Any) -> Dict[str, Any]:
|
||||
"""读当前工具参数，归一化成普通 dict。
|
||||
|
||||
返回 ``{"mass_kg": float, "com_m": [x,y,z], "tcp_m": [x,y,z]}``。
|
||||
读不到的字段用 0 / 空列表。
|
||||
"""
|
||||
try:
|
||||
params = tool.params() if group is None else tool.params(group)
|
||||
except Exception:
|
||||
return {"mass_kg": 0.0, "com_m": [], "tcp_m": []}
|
||||
|
||||
def _floats(value: Any) -> List[float]:
|
||||
try:
|
||||
return [float(v) for v in list(value)[:3]]
|
||||
except Exception:
|
||||
return []
|
||||
|
||||
return {
|
||||
"mass_kg": float(getattr(params, "mass", 0.0) or 0.0),
|
||||
"com_m": _floats(getattr(params, "CoM", []) or []),
|
||||
"tcp_m": _floats(getattr(params, "tcp_location", []) or []),
|
||||
}
|
||||
|
||||
|
||||
def iface_ipv4s(name: str) -> List[str]:
|
||||
"""网卡名 → 其上的 IPv4。FastDDS 白名单只认地址，不认 ``eth0`` / ``enx…``。"""
|
||||
import fcntl
|
||||
import socket
|
||||
import struct
|
||||
import subprocess
|
||||
|
||||
name = (name or "").strip()
|
||||
if not name:
|
||||
return []
|
||||
if name.count(".") == 3:
|
||||
return [name]
|
||||
addrs: List[str] = []
|
||||
try:
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
ifreq = struct.pack("256s", name.encode("utf-8")[:15])
|
||||
addrs.append(socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, ifreq)[20:24]))
|
||||
s.close()
|
||||
except OSError:
|
||||
pass
|
||||
try:
|
||||
out = subprocess.check_output(
|
||||
["ip", "-4", "-o", "addr", "show", "dev", name],
|
||||
text=True,
|
||||
stderr=subprocess.DEVNULL,
|
||||
)
|
||||
for line in out.splitlines():
|
||||
parts = line.split()
|
||||
if "inet" in parts:
|
||||
addrs.append(parts[parts.index("inet") + 1].split("/")[0])
|
||||
except Exception:
|
||||
pass
|
||||
seen = set()
|
||||
uniq: List[str] = []
|
||||
for a in addrs:
|
||||
if a and a not in seen:
|
||||
seen.add(a)
|
||||
uniq.append(a)
|
||||
return uniq
|
||||
|
||||
|
||||
def rdk_whitelist_lists(iface: str = "", pc_ip: str = "") -> List[List[str]]:
|
||||
"""给 ``flexivrdk.Robot(sn, listen)`` 用的白名单候选。
|
||||
|
||||
只放 IPv4。把 ``eth0`` 这种网卡名塞进去，FastDDS 会把真地址全部滤掉，
|
||||
症状是 ping 通、RDK 报 Failed to connect。
|
||||
"""
|
||||
lists: List[List[str]] = []
|
||||
pc = (pc_ip or os.environ.get("SITE_PC_IP") or "").strip()
|
||||
if pc.count(".") == 3:
|
||||
lists.append([pc])
|
||||
iface = (iface or "").strip()
|
||||
if iface.count(".") == 3:
|
||||
lists.append([iface])
|
||||
elif iface:
|
||||
lan = [a for a in iface_ipv4s(iface) if a.startswith("192.168.2.")]
|
||||
if lan:
|
||||
lists.append(lan)
|
||||
all_ips = iface_ipv4s(iface)
|
||||
if all_ips and all_ips != lan:
|
||||
lists.append(all_ips)
|
||||
seen = set()
|
||||
out: List[List[str]] = []
|
||||
for item in lists:
|
||||
key = tuple(item)
|
||||
if key not in seen:
|
||||
seen.add(key)
|
||||
out.append(item)
|
||||
return out
|
||||
|
||||
|
||||
def switch_tool(tool: Any, group: Any, target: str) -> None:
|
||||
"""切换工具。失败抛异常。"""
|
||||
if group is None:
|
||||
tool.Switch(target)
|
||||
else:
|
||||
tool.Switch(group, target)
|
||||
|
||||
|
||||
def resolve_tool_name(tool: Any, want: str) -> str:
|
||||
"""把用户给的模糊名字解析成 Studio 工具池里的准确名字。
|
||||
|
||||
大小写不敏感。找不到时抛 ``ValueError``，消息里列出全部可选项 ——
|
||||
这样操作员能立刻看到拼错在哪，而不是面对一个"切换失败"。
|
||||
"""
|
||||
want = (want or "").strip()
|
||||
if not want:
|
||||
return ""
|
||||
try:
|
||||
names = list(tool.list() or [])
|
||||
except Exception as exc: # noqa: BLE001
|
||||
raise ValueError(f"无法读取 Studio 工具列表: {exc}") from exc
|
||||
|
||||
if want in names:
|
||||
return want
|
||||
lower = {str(n).lower(): str(n) for n in names}
|
||||
hit = lower.get(want.lower())
|
||||
if hit:
|
||||
return hit
|
||||
raise ValueError(
|
||||
f"工具 '{want}' 不在 Studio 工具池里。可选: "
|
||||
+ (", ".join(str(n) for n in names) if names else "（空）")
|
||||
)
|
||||
Executable
+139
@@ -0,0 +1,139 @@
|
||||
#!/usr/bin/env bash
|
||||
# =============================================================================
|
||||
# scripts/lib/real_procs.sh —— 真机进程扫描（被 stop_real.sh / run_real.sh 共用）
|
||||
#
|
||||
# 为什么要有这个文件
|
||||
# ----------------
|
||||
# 仿真侧早就有 lib/sim_procs.sh（点名表 + install_sim/ 兜底 + 杀完复查），
|
||||
# 真机侧**一直没有**对应物：只有 run_real.sh 里两行 pkill，
|
||||
# pkill -f '/controller_manager/ros2_control_node'
|
||||
# pkill -f 'arm_bringup.*stack.launch'
|
||||
# 而真机栈里还有一族**独立于这两条存活**的进程。2026-09-28 实测抓到的一次：
|
||||
# 上一轮栈的主体早就没了，只剩
|
||||
# real_trajectory_adapter / named_pose_server / arm_status_node /
|
||||
# joint_state_publisher
|
||||
# 四条（全在 install/arm_control/lib/arm_control/ 下），两条 pkill 一条都匹配不到。
|
||||
#
|
||||
# 危害不是"占内存"，而是**冒名顶替**：
|
||||
# · 残留的 adapter 照样订阅 /trajectory、
|
||||
# · 残留的 named_pose_server 照样提供 /set_named_pose、
|
||||
# · 残留的 joint_state_publisher 照样发 /joint_states（mock 值的**静态**关节角），
|
||||
# 于是新一轮的请求被 DDS 负载均衡到"残留的那个"，/joint_states 与 /tf 有两份
|
||||
# 来源互相盖。现象极具误导性：控制器 feedback 与 reference 完全一致（控制器没坏），
|
||||
# 但 TF 显示末端**一动不动** —— 顺着现象查会一路查到 IK / TF / 时钟上。
|
||||
#
|
||||
# 所以这里做三件事：
|
||||
# 1. run_real.sh 与 stop_real.sh 共用同一份扫描实现（不再各写一份会漂移的表）；
|
||||
# 2. 扫描 = 点名表 + `install/` 前缀兜底 —— 凡命令行含 ``<仓库>/install/``
|
||||
# 的进程都算真机进程，以后再加新节点，不改这个文件也能被扫到；
|
||||
# 3. **按 ROS_DOMAIN_ID 区分真机与仿真**：仿真强制 77（见 sim_env.sh），
|
||||
# 真机默认 0。只按名字扫的话，停真机会顺手把正在跑的仿真的
|
||||
# robot_state_publisher 一起杀掉（两边同名、都在 conda/ROS 下）。
|
||||
#
|
||||
# 用法（在 `set -uo pipefail` 之后 source）：
|
||||
# source "$(dirname "$0")/lib/real_procs.sh"
|
||||
# mapfile -t targets < <(real_proc_targets) # 只输出 PID（已排除自身/祖先/仿真）
|
||||
# real_proc_scan # "PID<TAB>域<TAB>命令行"
|
||||
# =============================================================================
|
||||
|
||||
# 仿真的域（sim_env.sh 强制那个值）。真机扫到它 = 那是仿真进程，不是残留。
|
||||
REAL_SIM_DOMAIN=77
|
||||
|
||||
_real_repo_root() {
|
||||
cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd
|
||||
}
|
||||
|
||||
# 兜底模式：真机安装空间。用绝对路径前缀，避免误伤 install_sim/（仿真那边）。
|
||||
# 注意 "install_sim/" **不含** 子串 "install/"（_sim 把它截断了），所以这一条
|
||||
# 天然不会扫到仿真进程。
|
||||
#
|
||||
# ⚠️ 路径要按 awk 正则转义：路径里可能有 . + - 之类字符，不转义的话
|
||||
# "." 会匹配任意字符（匹配面变大 = 误杀无关进程）。
|
||||
_real_install_dir() {
|
||||
printf '%s/install/' "$(_real_repo_root)"
|
||||
}
|
||||
|
||||
_real_regex_escape() {
|
||||
printf '%s' "$1" | sed -e 's/[][\.^$*+?(){}|\\]/\\&/g'
|
||||
}
|
||||
|
||||
# 点名表：列**不在 install/ 下**、但属于真机栈的进程。
|
||||
# 它们的可执行文件来自 conda 环境或 ROS 发行版（/opt/ros/humble/lib/...），
|
||||
# 命令行里没有仓库的 install/ 字样，兜底规则扫不到。
|
||||
#
|
||||
# ros2_control_node 控制器本体（它占着 RDK，RDK 对同一台臂是独占的）
|
||||
# controller_manager.*spawner 孤儿 spawner，会永远等一个不存在的 controller_manager
|
||||
# joint_state_publisher **mock 专用**节点；在真机上出现 = 上一轮 mock 栈的残留，
|
||||
# 它发的静态关节角会盖掉真实值
|
||||
# robot_state_publisher 发 TF 与 /robot_description；残留会让"改了描述却不生效"
|
||||
# static_transform_publisher 发 latched 的 /tf_static，残留会一直把旧外参按在新树上
|
||||
REAL_PROC_NAMED='ros2_control_node|controller_manager.*spawner|joint_state_publisher|robot_state_publisher|static_transform_publisher'
|
||||
|
||||
real_proc_patterns() {
|
||||
printf '%s|%s' "$REAL_PROC_NAMED" "$(_real_regex_escape "$(_real_install_dir)")"
|
||||
}
|
||||
|
||||
# 读某个进程的 ROS_DOMAIN_ID。读不到（僵尸/无权限/没设）输出空。
|
||||
# `|| true`：调用方可能是 `set -e` 的脚本（run_real.sh），而 tr/sed 在
|
||||
# /proc 读不到时会非零返回 —— 不兜住的话整个启动脚本会被这一行带停。
|
||||
real_proc_domain() {
|
||||
local pid="$1" v=""
|
||||
v="$(tr '\0' '\n' < "/proc/${pid}/environ" 2>/dev/null \
|
||||
| sed -n 's/^ROS_DOMAIN_ID=//p' | head -1)" || true
|
||||
printf '%s' "${v:-}"
|
||||
}
|
||||
|
||||
# 是不是仿真进程（域 == REAL_SIM_DOMAIN）。
|
||||
# 读不到 environ 时**不当**成仿真：宁可停真机时多杀一个"域未知但确实在
|
||||
# install/ 下"的进程，也不要漏掉真机残留（漏掉的代价见文件头）。
|
||||
real_proc_is_sim() {
|
||||
[ "$(real_proc_domain "$1")" = "${REAL_SIM_DOMAIN}" ]
|
||||
}
|
||||
|
||||
# 扫描：输出 "PID<TAB>域<TAB>命令行"。
|
||||
#
|
||||
# ⚠️ 两个必须踩过的坑（与 sim_procs.sh 同源，别再犯）：
|
||||
# 1. `pkill -f <模式>` 会匹配到自己所在的 shell（它的命令行里就含那个模式），
|
||||
# 表现成"命令立刻返回、什么都没杀"。
|
||||
# 2. 模式串用**环境变量**传给 awk，不用 `-v pat=...`：用了 -v 的话 awk 自己的
|
||||
# 命令行里就含整串模式，于是它匹配到自己，复查时多报一个幽灵 PID。
|
||||
real_proc_scan() {
|
||||
export PAT="$(real_proc_patterns)"
|
||||
export ME="${REAL_PROC_SELF:-$$}"
|
||||
ps -eo pid=,ppid=,args= | awk '
|
||||
BEGIN { pat = ENVIRON["PAT"]; me = ENVIRON["ME"] }
|
||||
$0 ~ pat {
|
||||
if (NF < 3) next # 没有 args 字段（僵尸/内核线程）
|
||||
pid = $1; ppid = $2
|
||||
args = substr($0, index($0, $3))
|
||||
if (ppid == me) next # 本脚本的直接子进程（如 $(...) 的 fork）
|
||||
if (args ~ /real_procs\.sh/) next # 本库自己（含 grep/sed 的读数）
|
||||
if (args ~ /stop_real\.sh/) next
|
||||
if (args ~ /run_real\.sh/) next
|
||||
if (args ~ /^\s*awk/) next
|
||||
print pid "\t" args
|
||||
}'
|
||||
}
|
||||
|
||||
# 排除自己与全部祖先，跳过仿真进程，只输出 PID。
|
||||
# 设 REAL_PROC_INCLUDE_SIM=1 可以连仿真进程一起列出（自查用，停止流程不用）。
|
||||
real_proc_targets() {
|
||||
local exclude=() p=$$ pid rest _d skip e
|
||||
while [ "${p:-0}" -gt 1 ] 2>/dev/null; do
|
||||
exclude+=("$p")
|
||||
p=$(ps -o ppid= -p "$p" 2>/dev/null | tr -d ' ')
|
||||
[ -z "$p" ] && break
|
||||
done
|
||||
while IFS=$'\t' read -r pid rest; do
|
||||
[ -z "${pid:-}" ] && continue
|
||||
skip=0
|
||||
for e in "${exclude[@]}"; do
|
||||
[ "$pid" = "$e" ] && skip=1 && break
|
||||
done
|
||||
[ "$skip" = "1" ] && continue
|
||||
if [ "${REAL_PROC_INCLUDE_SIM:-0}" != "1" ] && real_proc_is_sim "$pid"; then
|
||||
continue
|
||||
fi
|
||||
echo "$pid"
|
||||
done < <(real_proc_scan)
|
||||
}
|
||||
Executable
+48
@@ -0,0 +1,48 @@
|
||||
#!/usr/bin/env bash
|
||||
# =============================================================================
|
||||
# scripts/lib/ros_domain.sh —— 真机与仿真的 DDS 域隔离
|
||||
#
|
||||
# 为什么要单独抽一层
|
||||
# ----------------
|
||||
# 仿真侧（scripts/sim_env.sh）**强制** ROS_DOMAIN_ID=77，并且注释里写明了
|
||||
# "隔离不能靠『环境里恰好没设』"。真机侧当时没有对应保护，于是同一个坑换个
|
||||
# 方向又踩了一次（2026-09-28 实测）：
|
||||
#
|
||||
# 先 source 过 sim_env.sh 的 shell（ROS_DOMAIN_ID=77 还 export 着）
|
||||
# 再起真机栈 → 真机栈落到**仿真的域**上 → 两边节点名撞车
|
||||
# （/controller_manager、/robot_state_publisher、/arm_status_node…）、
|
||||
# /joint_states 与 /tf 各有两个来源互相盖。
|
||||
#
|
||||
# 现象极具误导性：控制器 feedback 与 reference 完全一致（控制器没坏），
|
||||
# 但 TF 显示末端**一动不动** —— 顺着现象查会一路查到 IK / TF / 时钟上，
|
||||
# 全是无辜的。
|
||||
#
|
||||
# 真机侧有两个入口（scripts/env.sh 给交互 shell 用；scripts/run_real.sh 自己
|
||||
# 起 conda + install），两处都得有这个纠正，所以放在这里共用一份。
|
||||
#
|
||||
# 用法（在 `set -uo pipefail` 或 `set -eo pipefail` 之后 source）：
|
||||
# source ".../lib/ros_domain.sh"
|
||||
# ros_domain_force_real
|
||||
# =============================================================================
|
||||
|
||||
# 仿真的域。改这里要同步 scripts/sim_env.sh（那边是硬编码的 77）。
|
||||
ROS_SIM_DOMAIN=77
|
||||
|
||||
# 把 ROS_DOMAIN_ID 纠正到"真机该用的域"。
|
||||
# · REAL_ROS_DOMAIN_ID 显式设了 → 用它（要换域就走这个，名字里带 REAL，
|
||||
# 不会被仿真环境的 export 撞上）；
|
||||
# · 环境里是 77（仿真的域）→ **喊出来**再改回 0；
|
||||
# · 环境里没设 → 显式写 0，让人一眼能从 `env` 里看出真机跑在哪个域。
|
||||
ros_domain_force_real() {
|
||||
if [ -n "${REAL_ROS_DOMAIN_ID:-}" ]; then
|
||||
export ROS_DOMAIN_ID="${REAL_ROS_DOMAIN_ID}"
|
||||
elif [ "${ROS_DOMAIN_ID:-0}" = "${ROS_SIM_DOMAIN}" ]; then
|
||||
echo "[domain] ⚠ 环境里的 ROS_DOMAIN_ID=${ROS_SIM_DOMAIN} 是**仿真**用的域" >&2
|
||||
echo "[domain] （scripts/sim_env.sh 强制那个值）。已改回 0 起真机栈 ——" >&2
|
||||
echo "[domain] 真机与仿真同域会让节点名/话题互相盖，现象是'臂没动'" >&2
|
||||
echo "[domain] 而控制器一切正常。要另指定请设 REAL_ROS_DOMAIN_ID。" >&2
|
||||
export ROS_DOMAIN_ID=0
|
||||
elif [ -z "${ROS_DOMAIN_ID:-}" ]; then
|
||||
export ROS_DOMAIN_ID=0
|
||||
fi
|
||||
}
|
||||
Executable
+567
@@ -0,0 +1,567 @@
|
||||
#!/usr/bin/env python3
|
||||
"""RDK 控制桥（必须在 rdk_py310 / Python 3.10 下运行）。
|
||||
|
||||
为什么需要这个进程
|
||||
------------------
|
||||
非夕的 `flexivrdk` Python 绑定与 ROS 环境的 Python 3.11 ABI 不兼容，且 RDK 对
|
||||
同一台机器人是**独占访问**：多个进程同时连同一个 SN 会失败。所以框架里所有
|
||||
RDK 操作都收敛到本进程，由它串行化，业务节点通过 JSON 行协议远程调用。
|
||||
|
||||
这也让 `arm_driver_flexiv` 不必 import flexivrdk，符合
|
||||
`docs/INTERFACES.md` 的约束（业务节点不得 import flexivrdk）。
|
||||
|
||||
软拖拽为什么用 NRT_JOINT_IMPEDANCE
|
||||
----------------------------------
|
||||
AUTO(Remote) 模式下示教器上的 Free Drive 按钮不可用（控制权在 Remote）。
|
||||
RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。
|
||||
所以默认用低刚度 NRT_JOINT_IMPEDANCE：目标跟踪实测关节角 + 低刚度，
|
||||
于是手臂可以用手推动，同时 Remote 保持开启。
|
||||
|
||||
线程安全
|
||||
--------
|
||||
`flexivrdk.Robot` 不是线程安全的。所有 robot API 调用（含拖拽循环）共用同一
|
||||
把锁 `rlock`。拖拽期间的关节读取走缓存，避免 Enter 与 Stop() 竞争。
|
||||
|
||||
行协议（JSON 只写 stdout；RDK 自身日志走 stderr）
|
||||
-------------------------------------------------
|
||||
{"cmd":"ping"}
|
||||
{"cmd":"connect","sn":"<site.yaml 的 robot.sn>","iface":"<robot.iface>"}
|
||||
{"cmd":"enable"} # 使能 + 清故障（幂等）
|
||||
{"cmd":"clear_fault"} # 显式清故障并使能
|
||||
{"cmd":"switch_tool","name":"tool1"} # 切工具；name 空=只查询
|
||||
{"cmd":"status"} # operational/fault/mode/tool
|
||||
{"cmd":"drag_on","mode":"soft"} # soft | floating
|
||||
{"cmd":"drag_off"}
|
||||
{"cmd":"joints"}
|
||||
{"cmd":"move_joints","q":[0,0,0,0,0,0,0],"speed":0.15}
|
||||
{"cmd":"quit"}
|
||||
|
||||
每个命令的应答都是单行 JSON，含 ``ok`` 字段；失败时另有 ``error``。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
# 公共 RDK 操作统一在 scripts/lib/rdk_helpers.py，避免与 rdk_switch_tool.py
|
||||
# 各写一份导致行为漂移。
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parent / 'lib'))
|
||||
from rdk_helpers import ( # noqa: E402
|
||||
clear_fault as rdk_clear_fault,
|
||||
ensure_idle,
|
||||
ensure_operational,
|
||||
rdk_whitelist_lists,
|
||||
resolve_tool_name,
|
||||
single_arm_groups,
|
||||
switch_tool,
|
||||
tool_name,
|
||||
tool_params,
|
||||
)
|
||||
|
||||
_JSON_OUT = sys.stdout
|
||||
sys.stdout = sys.stderr
|
||||
|
||||
|
||||
def reply(obj: dict) -> None:
|
||||
_JSON_OUT.write(json.dumps(obj, ensure_ascii=False) + '\n')
|
||||
_JSON_OUT.flush()
|
||||
|
||||
|
||||
def log(msg: str) -> None:
|
||||
print(f'[rdk_bridge] {msg}', file=sys.stderr, flush=True)
|
||||
|
||||
|
||||
class SoftDrag:
|
||||
def __init__(self, robot, flexivrdk, lock: threading.Lock) -> None:
|
||||
self.robot = robot
|
||||
self.flexivrdk = flexivrdk
|
||||
self._lock = lock
|
||||
self._stop = threading.Event()
|
||||
self._thread: Optional[threading.Thread] = None
|
||||
self.latest_q: List[float] = []
|
||||
self._groups = {}
|
||||
|
||||
def start(self) -> None:
|
||||
mode = self.flexivrdk.Mode
|
||||
robot = self.robot
|
||||
with self._lock:
|
||||
robot.SwitchMode(mode.NRT_JOINT_IMPEDANCE)
|
||||
st = robot.states()
|
||||
self.latest_q = [float(v) for v in list(st.q)[:7]]
|
||||
try:
|
||||
info = robot.info()
|
||||
self._groups = dict(getattr(info, 'single_arm_groups', None) or {})
|
||||
except Exception as exc:
|
||||
log(f'info.single_arm_groups: {exc}')
|
||||
self._groups = {}
|
||||
if self._groups:
|
||||
for g in self._groups:
|
||||
try:
|
||||
nom = list(info.K_q_nom[g])
|
||||
soft = [max(8.0, 0.08 * float(v)) for v in nom]
|
||||
robot.SetJointImpedance(g, soft)
|
||||
log(f'SetJointImpedance {g} K={soft}')
|
||||
except Exception as exc:
|
||||
log(f'SetJointImpedance 组 {g}: {exc}')
|
||||
else:
|
||||
n = max(7, len(self.latest_q))
|
||||
try:
|
||||
robot.SetJointImpedance([35.0] * n, [0.7] * n)
|
||||
except Exception as exc:
|
||||
log(f'SetJointImpedance flat（不分组）: {exc}')
|
||||
|
||||
self._stop.clear()
|
||||
self._thread = threading.Thread(target=self._loop, name='soft-drag', daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
def _loop(self) -> None:
|
||||
robot = self.robot
|
||||
flexivrdk = self.flexivrdk
|
||||
while not self._stop.is_set():
|
||||
try:
|
||||
with self._lock:
|
||||
if self._stop.is_set():
|
||||
break
|
||||
st = robot.states()
|
||||
q = [float(v) for v in list(st.q)[:7]]
|
||||
self.latest_q = q
|
||||
if self._groups:
|
||||
cmds = {}
|
||||
for g in self._groups:
|
||||
try:
|
||||
gq = list(st[g].q)
|
||||
except Exception:
|
||||
gq = q
|
||||
n = len(gq)
|
||||
cmds[g] = flexivrdk.NrtJointPositionCmd(
|
||||
gq, [0.0] * n, [1.5] * n, [2.0] * n
|
||||
)
|
||||
robot.SendJointPosition(cmds)
|
||||
else:
|
||||
n = len(q)
|
||||
robot.SendJointPosition(q, [0.0] * n, [1.5] * n, [2.0] * n)
|
||||
except Exception as exc:
|
||||
log(f'soft 循环: {exc}')
|
||||
time.sleep(0.15)
|
||||
# 约 20 Hz —— 大部分时间让锁保持释放状态
|
||||
for _ in range(10):
|
||||
if self._stop.is_set():
|
||||
return
|
||||
time.sleep(0.005)
|
||||
|
||||
def stop(self) -> None:
|
||||
self._stop.set()
|
||||
th = self._thread
|
||||
if th is not None:
|
||||
th.join(timeout=4.0)
|
||||
if th.is_alive():
|
||||
log('join 之后 soft 循环仍然存活（无论如何都会切到 IDLE）')
|
||||
self._thread = None
|
||||
|
||||
|
||||
def _ensure_operational(robot) -> Optional[str]:
|
||||
"""保留旧名以兼容既有调用点；实现已统一到 rdk_helpers.ensure_operational。"""
|
||||
return ensure_operational(robot)
|
||||
|
||||
|
||||
def _restore_tracking_impedance(robot, flexivrdk) -> None:
|
||||
"""把关节阻抗从软拖拽拉回名义值，否则回放点位时臂跟不住指令。"""
|
||||
try:
|
||||
info = robot.info()
|
||||
groups = dict(getattr(info, 'single_arm_groups', None) or {})
|
||||
except Exception as exc:
|
||||
log(f'读名义阻抗失败: {exc}')
|
||||
groups = {}
|
||||
if groups:
|
||||
for group in groups:
|
||||
try:
|
||||
nominal = [float(v) for v in list(info.K_q_nom[group])]
|
||||
robot.SetJointImpedance(group, nominal)
|
||||
log(f'恢复阻抗 {group} K={nominal}')
|
||||
except Exception as exc:
|
||||
log(f'恢复阻抗 {group}: {exc}')
|
||||
return
|
||||
try:
|
||||
robot.SetJointImpedance([350.0] * 7, [7.0] * 7)
|
||||
except Exception as exc:
|
||||
log(f'恢复阻抗: {exc}')
|
||||
|
||||
|
||||
def _send_joint_target(robot, flexivrdk, target: List[float], grouped: Optional[bool]) -> bool:
|
||||
"""下发 7 关节目标。返回是否走了分组接口。"""
|
||||
if grouped is True:
|
||||
_send_grouped_joint_target(robot, flexivrdk, target)
|
||||
return True
|
||||
try:
|
||||
n = len(target)
|
||||
robot.SendJointPosition(target, [0.0] * n, [1.5] * n, [2.0] * n)
|
||||
return False
|
||||
except Exception as exc:
|
||||
if grouped is False:
|
||||
raise
|
||||
log(f'扁平关节指令失败，改分组: {exc}')
|
||||
_send_grouped_joint_target(robot, flexivrdk, target)
|
||||
return True
|
||||
|
||||
|
||||
def _send_grouped_joint_target(robot, flexivrdk, target: List[float]) -> None:
|
||||
info = robot.info()
|
||||
groups = dict(getattr(info, 'single_arm_groups', None) or {})
|
||||
if not groups:
|
||||
raise RuntimeError('没有分组，扁平关节指令也失败了')
|
||||
states = robot.states()
|
||||
cmds = {}
|
||||
placed = False
|
||||
for group in groups:
|
||||
try:
|
||||
current = [float(v) for v in list(states[group].q)]
|
||||
except Exception:
|
||||
current = list(target)
|
||||
use = target if (not placed and len(current) == len(target)) else current
|
||||
if use is target:
|
||||
placed = True
|
||||
n = len(use)
|
||||
cmds[group] = flexivrdk.NrtJointPositionCmd(use, [0.0] * n, [1.5] * n, [2.0] * n)
|
||||
if not placed:
|
||||
raise RuntimeError('没有一组关节数和示教点一致，拒绝下发')
|
||||
robot.SendJointPosition(cmds)
|
||||
|
||||
|
||||
def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool:
|
||||
"""切到 IDLE，同时避免把 stdin 循环永久阻塞住。"""
|
||||
done = threading.Event()
|
||||
|
||||
def run() -> None:
|
||||
try:
|
||||
acquired = lock.acquire(timeout=2.0)
|
||||
try:
|
||||
if acquired:
|
||||
try:
|
||||
robot.Stop()
|
||||
except Exception as exc:
|
||||
log(f'Stop: {exc}')
|
||||
try:
|
||||
robot.SwitchMode(flexivrdk.Mode.IDLE)
|
||||
except Exception as exc:
|
||||
log(f'SwitchMode IDLE: {exc}')
|
||||
finally:
|
||||
if acquired:
|
||||
lock.release()
|
||||
finally:
|
||||
done.set()
|
||||
|
||||
threading.Thread(target=run, name='idle', daemon=True).start()
|
||||
ok = done.wait(timeout)
|
||||
if not ok:
|
||||
log('IDLE/Stop 超时 —— stdin 循环继续运行')
|
||||
return ok
|
||||
|
||||
|
||||
def main() -> int:
|
||||
import flexivrdk
|
||||
|
||||
robot = None
|
||||
drag_on = False
|
||||
soft: Optional[SoftDrag] = None
|
||||
rlock = threading.Lock()
|
||||
cached_q: List[float] = []
|
||||
|
||||
for line in sys.stdin:
|
||||
line = line.strip()
|
||||
if not line:
|
||||
continue
|
||||
try:
|
||||
msg = json.loads(line)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
reply({'ok': False, 'error': f'JSON 解析失败: {exc}'})
|
||||
continue
|
||||
|
||||
cmd = str(msg.get('cmd', '')).strip()
|
||||
try:
|
||||
if cmd == 'ping':
|
||||
reply({'ok': True, 'py': sys.version.split()[0]})
|
||||
|
||||
elif cmd == 'connect':
|
||||
sn = str(msg.get('sn') or os.environ.get('SITE_ROBOT_SN') or '')
|
||||
iface = str(msg.get('iface') or '')
|
||||
robot = None
|
||||
last_err = None
|
||||
attempts = []
|
||||
# 白名单必须是 IPv4。传 eth0 / enx… 会被 FastDDS 整段滤掉，
|
||||
# ping 通也会 Failed to connect。
|
||||
for listen in rdk_whitelist_lists(iface):
|
||||
log('白名单: ' + ', '.join(listen))
|
||||
attempts.extend([(sn, listen), (sn, listen, True)])
|
||||
attempts.append((sn,))
|
||||
for args in attempts:
|
||||
try:
|
||||
robot = flexivrdk.Robot(*args)
|
||||
log(f'已连接 {sn} args={args!r}')
|
||||
last_err = None
|
||||
break
|
||||
except TypeError:
|
||||
continue
|
||||
except Exception as exc:
|
||||
last_err = exc
|
||||
log(f'连接 {args!r}: {exc}')
|
||||
robot = None
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': str(last_err or '连接失败')})
|
||||
continue
|
||||
err = _ensure_operational(robot)
|
||||
if err:
|
||||
reply({'ok': False, 'error': err})
|
||||
continue
|
||||
with rlock:
|
||||
q = [float(v) for v in list(robot.states().q)[:7]]
|
||||
cached_q = q
|
||||
reply({'ok': True, 'sn': sn, 'q': q})
|
||||
|
||||
elif cmd == 'drag_on':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': '尚未连接'})
|
||||
continue
|
||||
err = _ensure_operational(robot)
|
||||
if err:
|
||||
reply({'ok': False, 'error': err})
|
||||
continue
|
||||
|
||||
prefer = str(msg.get('mode', 'soft')).strip().lower()
|
||||
if prefer in ('floating', 'primitive', 'floatingjoint'):
|
||||
name = str(msg.get('primitive', 'FloatingJoint')).replace('()', '')
|
||||
try:
|
||||
with rlock:
|
||||
robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
|
||||
robot.ExecutePrimitive(name, {})
|
||||
drag_on = True
|
||||
reply({'ok': True, 'drag': True, 'mode': 'floating', 'primitive': name})
|
||||
continue
|
||||
except Exception as exc:
|
||||
log(f'Floating 失败，退回 soft 模式: {exc}')
|
||||
|
||||
try:
|
||||
if soft is not None:
|
||||
soft.stop()
|
||||
soft = SoftDrag(robot, flexivrdk, rlock)
|
||||
soft.start()
|
||||
if soft.latest_q:
|
||||
cached_q = list(soft.latest_q)
|
||||
drag_on = True
|
||||
log('软拖拽已开启')
|
||||
reply({
|
||||
'ok': True,
|
||||
'drag': True,
|
||||
'mode': 'soft',
|
||||
'hint': '轻推机械臂即可，关节阻抗已调低。Enter=拍照（s=保持）。',
|
||||
})
|
||||
except Exception as exc:
|
||||
reply({'ok': False, 'error': f'软拖拽失败: {exc}'})
|
||||
|
||||
elif cmd == 'drag_off':
|
||||
log('收到 drag_off 请求')
|
||||
if soft is not None:
|
||||
if soft.latest_q:
|
||||
cached_q = list(soft.latest_q)
|
||||
soft.stop()
|
||||
soft = None
|
||||
if robot is not None:
|
||||
_idle_async(robot, rlock, flexivrdk, timeout=4.0)
|
||||
drag_on = False
|
||||
log('drag_off 已完成')
|
||||
reply({'ok': True, 'drag': False, 'q': cached_q})
|
||||
|
||||
elif cmd == 'joints':
|
||||
q: Optional[List[float]] = None
|
||||
if soft is not None and len(soft.latest_q) >= 7:
|
||||
q = list(soft.latest_q)
|
||||
elif len(cached_q) >= 7:
|
||||
q = list(cached_q)
|
||||
if q is None or len(q) < 7:
|
||||
reply({'ok': False, 'error': '没有关节缓存'})
|
||||
continue
|
||||
cached_q = q
|
||||
reply({'ok': True, 'q': q, 'drag': drag_on})
|
||||
|
||||
elif cmd == 'enable':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': '尚未连接'})
|
||||
continue
|
||||
err = ensure_operational(robot)
|
||||
reply({
|
||||
'ok': err is None,
|
||||
'error': err,
|
||||
'operational': bool(robot.operational()),
|
||||
})
|
||||
|
||||
elif cmd == 'clear_fault':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': '尚未连接'})
|
||||
continue
|
||||
err = rdk_clear_fault(robot)
|
||||
if err:
|
||||
reply({'ok': False, 'error': err})
|
||||
continue
|
||||
# 清故障后顺手使能，否则调用方还得再发一条 enable。
|
||||
err2 = ensure_operational(robot)
|
||||
reply({
|
||||
'ok': err2 is None,
|
||||
'error': err2,
|
||||
'fault': bool(robot.fault()),
|
||||
'operational': bool(robot.operational()),
|
||||
})
|
||||
|
||||
elif cmd == 'switch_tool':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': '尚未连接'})
|
||||
continue
|
||||
want = str(msg.get('name', '')).strip()
|
||||
|
||||
# 切换工具要求 IDLE；这一步是同步的，因为切换本身不能被打断。
|
||||
err = ensure_idle(robot, flexivrdk.Mode, timeout_s=12.0)
|
||||
if err:
|
||||
reply({'ok': False, 'error': err})
|
||||
continue
|
||||
|
||||
try:
|
||||
with rlock:
|
||||
tool = flexivrdk.Tool(robot)
|
||||
groups = single_arm_groups(robot)
|
||||
available = [str(n) for n in (tool.list() or [])]
|
||||
anchor = groups[0] if groups else None
|
||||
|
||||
if not want:
|
||||
# 纯查询：不切换，把当前工具与参数报回去。
|
||||
params = tool_params(tool, anchor)
|
||||
reply({
|
||||
'ok': True,
|
||||
'switched': False,
|
||||
'current': tool_name(tool, anchor),
|
||||
'available': available,
|
||||
**params,
|
||||
})
|
||||
continue
|
||||
|
||||
target = resolve_tool_name(tool, want)
|
||||
already = all(tool_name(tool, g) == target for g in groups)
|
||||
if not already:
|
||||
for g in groups:
|
||||
switch_tool(tool, g, target)
|
||||
time.sleep(0.3)
|
||||
|
||||
params = tool_params(tool, anchor)
|
||||
reply({
|
||||
'ok': True,
|
||||
'switched': not already,
|
||||
'already': already,
|
||||
'current': tool_name(tool, anchor),
|
||||
'target': target,
|
||||
'available': available,
|
||||
**params,
|
||||
})
|
||||
except Exception as exc: # noqa: BLE001
|
||||
reply({'ok': False, 'error': str(exc)})
|
||||
|
||||
elif cmd == 'status':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'connected': False, 'error': '尚未 connect'})
|
||||
continue
|
||||
info = {'ok': True, 'connected': True, 'drag': drag_on}
|
||||
try:
|
||||
info['operational'] = bool(robot.operational())
|
||||
info['fault'] = bool(robot.fault())
|
||||
info['mode'] = str(robot.mode())
|
||||
except Exception as exc: # noqa: BLE001
|
||||
info['error'] = str(exc)
|
||||
try:
|
||||
with rlock:
|
||||
tool = flexivrdk.Tool(robot)
|
||||
groups = single_arm_groups(robot)
|
||||
info['tool'] = tool_name(tool, groups[0] if groups else None)
|
||||
except Exception:
|
||||
info['tool'] = ''
|
||||
reply(info)
|
||||
|
||||
elif cmd == 'move_joints':
|
||||
if robot is None:
|
||||
reply({'ok': False, 'error': '尚未连接'})
|
||||
continue
|
||||
raw = msg.get('q')
|
||||
if not isinstance(raw, list) or len(raw) < 7:
|
||||
reply({'ok': False, 'error': '需要 7 个关节角（弧度）'})
|
||||
continue
|
||||
try:
|
||||
target = [float(v) for v in raw[:7]]
|
||||
except (TypeError, ValueError):
|
||||
reply({'ok': False, 'error': '关节角不是数字'})
|
||||
continue
|
||||
if any(v != v or abs(v) > 12.6 for v in target):
|
||||
reply({'ok': False, 'error': '关节角不是有限值，或超出约两圈'})
|
||||
continue
|
||||
try:
|
||||
speed = float(msg.get('speed', 0.15))
|
||||
except (TypeError, ValueError):
|
||||
speed = 0.15
|
||||
speed = max(0.05, min(0.35, speed))
|
||||
if soft is not None:
|
||||
soft.stop()
|
||||
soft = None
|
||||
drag_on = False
|
||||
err = _ensure_operational(robot)
|
||||
if err:
|
||||
reply({'ok': False, 'error': err})
|
||||
continue
|
||||
try:
|
||||
with rlock:
|
||||
robot.SwitchMode(flexivrdk.Mode.NRT_JOINT_IMPEDANCE)
|
||||
current = [float(v) for v in list(robot.states().q)[:7]]
|
||||
_restore_tracking_impedance(robot, flexivrdk)
|
||||
max_delta = max(abs(a - b) for a, b in zip(current, target))
|
||||
step_s = 0.05
|
||||
steps = max(1, int(max_delta / (speed * step_s)))
|
||||
if steps * step_s > 150.0:
|
||||
reply({
|
||||
'ok': False,
|
||||
'error': f'路程太长（{max_delta:.2f} rad），请先拖近一点再回放',
|
||||
})
|
||||
continue
|
||||
grouped: Optional[bool] = None
|
||||
for index in range(1, steps + 1):
|
||||
alpha = index / steps
|
||||
sample = [a + (b - a) * alpha for a, b in zip(current, target)]
|
||||
with rlock:
|
||||
grouped = _send_joint_target(robot, flexivrdk, sample, grouped)
|
||||
time.sleep(step_s)
|
||||
for _hold in range(8):
|
||||
with rlock:
|
||||
grouped = _send_joint_target(robot, flexivrdk, target, grouped)
|
||||
time.sleep(step_s)
|
||||
cached_q = list(target)
|
||||
reply({'ok': True, 'q': target, 'from': current, 'steps': steps, 'speed': speed})
|
||||
except Exception as exc:
|
||||
reply({'ok': False, 'error': f'回点失败: {exc}'})
|
||||
|
||||
elif cmd == 'quit':
|
||||
if soft is not None:
|
||||
soft.stop()
|
||||
soft = None
|
||||
if robot is not None and drag_on:
|
||||
_idle_async(robot, rlock, flexivrdk, timeout=3.0)
|
||||
reply({'ok': True})
|
||||
return 0
|
||||
|
||||
else:
|
||||
reply({'ok': False, 'error': f'未知命令 {cmd}'})
|
||||
except Exception as exc: # noqa: BLE001
|
||||
log(f'cmd={cmd} 出错: {exc}')
|
||||
reply({'ok': False, 'error': str(exc)})
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main())
|
||||
Executable
+276
@@ -0,0 +1,276 @@
|
||||
#!/usr/bin/env python3
|
||||
"""把 Flexiv 当前工具（TCP + 负载）切换成 Studio 里配置好的名字。
|
||||
|
||||
必须在 RDK 的 Python（rdk_py310）下运行。机器人必须处于 IDLE；如果
|
||||
ros2_control 已经占住了 RDK 连接，请先停掉 run_real.sh。
|
||||
|
||||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --list
|
||||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --name Flange
|
||||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py # site.yaml robot.tool_name
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import time
|
||||
|
||||
|
||||
def _single_arm_groups(robot):
|
||||
"""RDK 1.9 的 Tool API 是单臂的（没有 JointGroup）。老版本用 groups。"""
|
||||
info = robot.info()
|
||||
groups = getattr(info, "single_arm_groups", None) or {}
|
||||
if isinstance(groups, dict):
|
||||
keys = list(groups.keys())
|
||||
else:
|
||||
keys = list(groups)
|
||||
if keys:
|
||||
return keys
|
||||
try:
|
||||
import flexivrdk
|
||||
|
||||
if hasattr(flexivrdk, "JointGroup"):
|
||||
return [flexivrdk.JointGroup.ARM_1]
|
||||
except Exception:
|
||||
pass
|
||||
return [None] # 哨兵：调用 Tool.name()/Switch() 时不带 group
|
||||
|
||||
|
||||
def _tool_name(tool, group):
|
||||
if group is None:
|
||||
return tool.name()
|
||||
return tool.name(group)
|
||||
|
||||
|
||||
def _tool_params(tool, group):
|
||||
if group is None:
|
||||
return tool.params()
|
||||
return tool.params(group)
|
||||
|
||||
|
||||
def _tool_switch(tool, group, target: str) -> None:
|
||||
if group is None:
|
||||
tool.Switch(target)
|
||||
return
|
||||
tool.Switch(group, target)
|
||||
|
||||
|
||||
def _group_name(group) -> str:
|
||||
if group is None:
|
||||
return "单臂"
|
||||
try:
|
||||
import flexivrdk
|
||||
|
||||
return str(flexivrdk.kJointGroupNames[group])
|
||||
except Exception:
|
||||
return str(group)
|
||||
|
||||
|
||||
def _fmt_params(params) -> str:
|
||||
mass = getattr(params, "mass", None)
|
||||
com = list(getattr(params, "CoM", []) or [])
|
||||
tcp = list(getattr(params, "tcp_location", []) or [])
|
||||
com_s = ",".join(f"{float(v):.4f}" for v in com[:3]) if com else "?"
|
||||
tcp_s = ",".join(f"{float(v):.4f}" for v in tcp[:3]) if tcp else "?"
|
||||
return f"mass={mass} kg CoM=[{com_s}] m TCP_xyz=[{tcp_s}] m"
|
||||
|
||||
|
||||
def _ensure_idle(robot, mode) -> None:
|
||||
try:
|
||||
robot.SwitchMode(mode.IDLE)
|
||||
return
|
||||
except Exception:
|
||||
pass
|
||||
if hasattr(robot, "Enable"):
|
||||
robot.Enable()
|
||||
else:
|
||||
robot.ServoOn()
|
||||
for _ in range(40):
|
||||
if robot.operational():
|
||||
break
|
||||
time.sleep(0.25)
|
||||
if not robot.operational():
|
||||
raise RuntimeError(
|
||||
"机器人未处于 operational 状态 —— 请在示教器切 AUTO Remote 并使能，松开急停"
|
||||
)
|
||||
robot.SwitchMode(mode.IDLE)
|
||||
|
||||
|
||||
def _resolve_name(tool, want: str) -> str:
|
||||
want = (want or "").strip()
|
||||
if not want:
|
||||
return ""
|
||||
names = list(tool.list() or [])
|
||||
if want in names:
|
||||
return want
|
||||
lower = {n.lower(): n for n in names}
|
||||
hit = lower.get(want.lower())
|
||||
if hit:
|
||||
return hit
|
||||
raise SystemExit(
|
||||
"工具 %r 不在 Studio 工具池里。\n 可选: %s"
|
||||
% (want, ", ".join(names) or "（空）")
|
||||
)
|
||||
|
||||
|
||||
def _iface_ipv4s(name: str) -> list:
|
||||
"""FastDDS 白名单要的是 IPv4 地址，而不是 enp4s0 这类网卡名。"""
|
||||
import socket
|
||||
import fcntl
|
||||
import struct
|
||||
|
||||
name = (name or "").strip()
|
||||
if not name:
|
||||
return []
|
||||
if name.count(".") == 3:
|
||||
return [name]
|
||||
addrs = []
|
||||
try:
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
ifreq = struct.pack("256s", name.encode("utf-8")[:15])
|
||||
addrs.append(socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, ifreq)[20:24]))
|
||||
s.close()
|
||||
except OSError:
|
||||
pass
|
||||
try:
|
||||
import subprocess
|
||||
|
||||
out = subprocess.check_output(
|
||||
["ip", "-4", "-o", "addr", "show", "dev", name],
|
||||
text=True,
|
||||
stderr=subprocess.DEVNULL,
|
||||
)
|
||||
for line in out.splitlines():
|
||||
parts = line.split()
|
||||
if "inet" in parts:
|
||||
cidr = parts[parts.index("inet") + 1]
|
||||
addrs.append(cidr.split("/")[0])
|
||||
except Exception:
|
||||
pass
|
||||
# 去重，保持原有顺序
|
||||
seen = set()
|
||||
uniq = []
|
||||
for a in addrs:
|
||||
if a and a not in seen:
|
||||
seen.add(a)
|
||||
uniq.append(a)
|
||||
return uniq
|
||||
|
||||
|
||||
def _rdk_whitelist(iface: str) -> list[list[str]]:
|
||||
"""只放 IPv4。网卡名进白名单会被 FastDDS 滤掉（见 rdk_helpers）。"""
|
||||
sys.path.insert(0, str(__import__("pathlib").Path(__file__).resolve().parent / "lib"))
|
||||
from rdk_helpers import rdk_whitelist_lists # noqa: PLC0415
|
||||
|
||||
return rdk_whitelist_lists(iface)
|
||||
|
||||
|
||||
def connect_rdk(sn: str, iface: str = ""):
|
||||
import flexivrdk
|
||||
|
||||
for listen in _rdk_whitelist(iface):
|
||||
print("RDK 白名单:", ", ".join(listen))
|
||||
for args in ((sn, listen), (sn, listen, True), (sn, listen, True, False)):
|
||||
try:
|
||||
return flexivrdk.Robot(*args)
|
||||
except TypeError:
|
||||
continue
|
||||
except RuntimeError as exc:
|
||||
msg = str(exc)
|
||||
if "whitelist" in msg.lower() or "filtered out" in msg.lower():
|
||||
print(" 白名单被拒，试下一个")
|
||||
break
|
||||
if "Failed to connect" in msg or "failed to connect" in msg.lower():
|
||||
print(" 用这个白名单搜索超时")
|
||||
break
|
||||
raise
|
||||
print("不带 iface 过滤再试一次")
|
||||
return flexivrdk.Robot(sn)
|
||||
|
||||
|
||||
def main() -> int:
|
||||
ap = argparse.ArgumentParser(description="切换 Flexiv Studio 工具 / TCP")
|
||||
ap.add_argument("--sn", default="", help="机械臂序列号（默认取 site.yaml）")
|
||||
ap.add_argument("--iface", default="", help="RDK 用的网卡白名单（如 enp4s0）")
|
||||
ap.add_argument(
|
||||
"--name",
|
||||
default="",
|
||||
help="Studio 工具名（Flange = 不装工具）。留空 = 用 site.yaml 的 robot.tool_name",
|
||||
)
|
||||
ap.add_argument("--list", action="store_true", help="列出工具后退出")
|
||||
args = ap.parse_args()
|
||||
|
||||
sn = (args.sn or "").strip()
|
||||
want = (args.name or "").strip()
|
||||
if not sn:
|
||||
print("错误：必须提供 --sn（请通过 run_real.sh / rdk_switch_tool.sh 运行）", file=sys.stderr)
|
||||
return 2
|
||||
|
||||
import flexivrdk
|
||||
|
||||
mode = flexivrdk.Mode
|
||||
iface = (args.iface or "").strip()
|
||||
print(f"=== RDK 工具 SN={sn} iface={iface or 'auto'} ===")
|
||||
robot = None
|
||||
try:
|
||||
robot = connect_rdk(sn, iface)
|
||||
if robot.fault():
|
||||
print("检测到故障，正在 ClearFault ...")
|
||||
if not robot.ClearFault():
|
||||
print("错误：故障无法清除", file=sys.stderr)
|
||||
return 1
|
||||
tool = flexivrdk.Tool(robot)
|
||||
names = list(tool.list() or [])
|
||||
groups = _single_arm_groups(robot)
|
||||
print("Studio 工具:", ", ".join(names) or "（无）")
|
||||
for g in groups:
|
||||
cur = _tool_name(tool, g)
|
||||
extra = ""
|
||||
try:
|
||||
extra = " " + _fmt_params(_tool_params(tool, g))
|
||||
except Exception:
|
||||
pass
|
||||
print(f" 当前 [{_group_name(g)}]: {cur}{extra}")
|
||||
|
||||
if args.list:
|
||||
return 0
|
||||
if not want:
|
||||
print("tool_name 为空 —— 保持当前工具（要切换请在 site.yaml 里设置 robot.tool_name）")
|
||||
print("RDK 正常")
|
||||
return 0
|
||||
|
||||
target = _resolve_name(tool, want)
|
||||
already = all(_tool_name(tool, g) == target for g in groups)
|
||||
if already:
|
||||
print(f"已经处于 [{target}]")
|
||||
print("RDK 正常")
|
||||
return 0
|
||||
|
||||
print(f"正在切换到 → [{target}]（需要 IDLE）")
|
||||
_ensure_idle(robot, mode)
|
||||
for g in groups:
|
||||
_tool_switch(tool, g, target)
|
||||
for g in groups:
|
||||
cur = _tool_name(tool, g)
|
||||
extra = ""
|
||||
try:
|
||||
extra = " " + _fmt_params(_tool_params(tool, g))
|
||||
except Exception:
|
||||
pass
|
||||
print(f" 现在 [{_group_name(g)}]: {cur}{extra}")
|
||||
if cur != target:
|
||||
print(f"错误：切换失败，当前仍是 {cur!r}", file=sys.stderr)
|
||||
return 1
|
||||
print("RDK 正常")
|
||||
return 0
|
||||
except Exception as exc:
|
||||
print(f"错误：{exc}", file=sys.stderr)
|
||||
print(" 请先停掉 run_real.sh（RDK 是独占的）。", file=sys.stderr)
|
||||
print(" 示教器：切 AUTO Remote，并使能 Ethernet/RDK。", file=sys.stderr)
|
||||
return 1
|
||||
finally:
|
||||
robot = None
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Executable
+288
@@ -0,0 +1,288 @@
|
||||
#!/usr/bin/env bash
|
||||
# 跟踪 → 对孔。搜孔轻压已取消，本脚本不再进入那一步，也不能用参数打开。
|
||||
#
|
||||
# 为什么要单独有这个脚本：
|
||||
# pipeline.launch.py 只管两件事 —— 发布手眼静态 TF、起 assembly_pipeline 串联节点。
|
||||
# 它**不**拉相机、也**不**拉触觉（见它文件头的注释）。前置的相机与触感服务
|
||||
# 必须由本脚本拉起一次，否则二/三阶段会因为拿不到图像或力数据而空转。
|
||||
#
|
||||
# 本脚本做的事（顺序固定）：
|
||||
# 1) 确认机械臂控制器已 ACTIVE（不在就直接报错退出，不假装能跑）
|
||||
# 2) 拉起 D405 相机（已在发图就跳过）
|
||||
# 3) 拉起触觉服务（只有左臂装了传感器；右臂自动跳过）
|
||||
# 4) exec pipeline.launch.py，把下面的 CLI 参数映射成它的 launch 参数
|
||||
#
|
||||
# 用法：
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 # 跟踪 + 对孔
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2 # 指定对 k2 孔
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1 # 臂已在工件上方，跳过跟踪
|
||||
# 旋入不在这里。用 run_stage3_search.sh --arm 89 --screw
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview # 相机带 Qt 预览窗
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera # 相机已在别处拉起了
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile # 不起触觉服务
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 对准后只降到观测高度（默认离面 15mm）
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 # 启动后先到这个法兰高度 [m]
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --hover-z 0.34 # 对孔悬停法兰高度 [m]，不安全会拒绝
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8 # 下臂锁 XY，规尖停在离面 8mm（site 默认 2）
|
||||
#
|
||||
# 前置（另开一个终端，先跑起来）：
|
||||
# bash ~/ros2/scripts/run_real.sh --arm 89
|
||||
#
|
||||
# 注意：run_pipeline.sh 是本脚本的别名（历史名字），两者等价。
|
||||
|
||||
set -eo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
|
||||
# ---- 默认值：全部沿用 site.yaml / launch 默认，CLI 只做覆盖 ----
|
||||
SKIP_STAGE1=false
|
||||
SKIP_STAGE2=false
|
||||
SKIP_STAGE3=true # 搜孔已取消。--skip-stage3 仍然接受，但没有开关能把它重新打开。
|
||||
HOLE=""
|
||||
DEPTH=""
|
||||
PITCH=""
|
||||
PIPELINE_PREVIEW=true # 传给 pipeline 节点：是否显示推理预览
|
||||
RESNAP=false
|
||||
WITH_CAMERA=true
|
||||
WITH_TACTILE=true
|
||||
CAMERA_PREVIEW=false
|
||||
NO_DESCENT=false
|
||||
INIT_Z=""
|
||||
HOVER_Z=""
|
||||
FINAL_GAP=""
|
||||
|
||||
# --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||||
parse_arm_args "$@"
|
||||
set -- "${PARSE_ARM_REST[@]}"
|
||||
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--skip-stage1) SKIP_STAGE1=true; shift ;;
|
||||
--skip-stage2) SKIP_STAGE2=true; shift ;;
|
||||
--skip-stage3) SKIP_STAGE3=true; shift ;;
|
||||
--hole) HOLE="${2:?--hole 需要一个孔名（k1 或 k2）}"; shift 2 ;;
|
||||
--depth) DEPTH="${2:?--depth 需要毫米数}"; shift 2 ;;
|
||||
--pitch) PITCH="${2:?--pitch 需要螺距毫米数}"; shift 2 ;;
|
||||
--resnap) RESNAP=true; shift ;;
|
||||
# 不到贴面 2mm。对准后仍降到 site 的 observe_gap_mm（默认离面 15mm），
|
||||
# 方便看规尖偏没偏；对不准就下到 2mm 会把规顶在孔沿上（2026-10-08 真机 63N）。
|
||||
--no-descent) NO_DESCENT=true; shift ;;
|
||||
--init-z) INIT_Z="${2:?--init-z 需要法兰高度，单位米}"; shift 2 ;;
|
||||
--hover-z) HOVER_Z="${2:?--hover-z 需要法兰高度，单位米}"; shift 2 ;;
|
||||
--final-gap) FINAL_GAP="${2:?--final-gap 需要规尖离面间隙，单位毫米}"; shift 2 ;;
|
||||
--no-preview) PIPELINE_PREVIEW=false; shift ;;
|
||||
--camera-preview) CAMERA_PREVIEW=true; shift ;;
|
||||
--no-camera) WITH_CAMERA=false; shift ;;
|
||||
--no-tactile) WITH_TACTILE=false; shift ;;
|
||||
-h|--help) sed -n '2,32p' "$0"; exit 0 ;;
|
||||
*) echo "Unknown: $1" >&2; exit 1 ;;
|
||||
esac
|
||||
done
|
||||
|
||||
if [[ -n "${DEPTH}" || -n "${PITCH}" ]]; then
|
||||
echo "ERROR: 搜孔轻压已取消，整体流程不再搜孔，也不在这里旋入。" >&2
|
||||
echo " 旋入请用: bash ~/ros2/scripts/run_stage3_search.sh --arm ${SITE_ROBOT_ARM:-89} --screw" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
if [[ -n "${FINAL_GAP}" && "${NO_DESCENT}" == true ]]; then
|
||||
echo "ERROR: 同时写了 --final-gap 和 --no-descent，不知道听谁的。" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
|
||||
if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
|
||||
export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}"
|
||||
export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}"
|
||||
fi
|
||||
|
||||
echo "=== run_all（三阶段串联） arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN:-?}) ==="
|
||||
|
||||
# ---- 1) 控制器必须在。判活方式与 run_stage2_locate.sh 保持一致 ----
|
||||
echo "=== 等待 rizon_arm_controller ACTIVE（T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}）==="
|
||||
# 先看服务在不在，再 call —— 顺序反过来的话，服务不存在时每次 call 都要耗满
|
||||
# 超时才返回，30 次循环能白等两三分钟（实测 63s），用户会以为脚本卡死了。
|
||||
ready=false
|
||||
if timeout 3 ros2 service list 2>/dev/null | grep -q "^/controller_manager/list_controllers$"; then
|
||||
for _i in $(seq 1 20); do
|
||||
if timeout 3 ros2 service call /controller_manager/list_controllers \
|
||||
controller_manager_msgs/srv/ListControllers 2>/dev/null \
|
||||
| grep -q "name='rizon_arm_controller', state='active'"; then
|
||||
ready=true
|
||||
break
|
||||
fi
|
||||
sleep 0.5
|
||||
done
|
||||
fi
|
||||
if [[ "${ready}" != true ]]; then
|
||||
echo "ERROR: 机械臂控制器未激活。请先在另一终端跑：" >&2
|
||||
echo " bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-89}" >&2
|
||||
echo "（若该终端已在跑，先确认它没报错退出、且 Rizon 上电使能成功。）" >&2
|
||||
exit 1
|
||||
fi
|
||||
echo " rizon_arm_controller ACTIVE"
|
||||
|
||||
# ---- 2) 相机。已在发图就跳过（避免重复起导致 USB 独占失败）----
|
||||
CAM_NS="${SITE_CAM_NAME:-d405_left}"
|
||||
IMG_TOPIC="/${CAM_NS}/color/image_raw"
|
||||
|
||||
_camera_ok() {
|
||||
# 以真实消息为准：topic info 的 Publisher count 依赖 DDS 发现，
|
||||
# 进程频繁重启后会出现假阴性（run_stage2_locate.sh 里踩过这个坑）。
|
||||
# 但先查 topic 列表，避免话题根本不存在时 echo 白等到超时。
|
||||
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${IMG_TOPIC}" || return 1
|
||||
timeout 3 ros2 topic echo --once "${IMG_TOPIC}" 2>/dev/null | grep -q "^header:"
|
||||
}
|
||||
|
||||
if [[ "${WITH_CAMERA}" == true ]]; then
|
||||
if _camera_ok; then
|
||||
echo "=== 相机 ${IMG_TOPIC} 已在发图，跳过拉起 ==="
|
||||
else
|
||||
echo "=== 拉起相机 ${CAM_NS} ==="
|
||||
# 后台拉起；默认不带预览窗（nohup 下开 Qt 窗口没有意义）。
|
||||
# 想实时看画面就用 --camera-preview，或干脆另开终端跑 run_camera_d405.sh --preview。
|
||||
CAM_ARGS=()
|
||||
[[ "${CAMERA_PREVIEW}" == true ]] && CAM_ARGS+=(--preview) || CAM_ARGS+=(--no-preview)
|
||||
nohup bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" \
|
||||
"${CAM_ARGS[@]}" > /tmp/run_all_camera.log 2>&1 &
|
||||
echo " camera pid=$! log=/tmp/run_all_camera.log"
|
||||
cam_ok=false
|
||||
for _i in $(seq 1 24); do # 最多等 ~12s；RealSense 枚举 + 首帧比想象中慢
|
||||
if _camera_ok; then cam_ok=true; break; fi
|
||||
sleep 0.5
|
||||
done
|
||||
if [[ "${cam_ok}" != true ]]; then
|
||||
echo "ERROR: ${IMG_TOPIC} 仍无图像。看日志：/tmp/run_all_camera.log" >&2
|
||||
echo " 常见原因：USB 没插好；或另一个进程占着相机（USB 是独占的）。" >&2
|
||||
exit 1
|
||||
fi
|
||||
echo " 相机 ${CAM_NS} 出图 OK"
|
||||
fi
|
||||
# 一阶段混合跟踪：D405 看不见时要马上拍 D435。顶视没起来就只能干等。
|
||||
D435_TOPIC="/d435/color/image_raw"
|
||||
_d435_ok() {
|
||||
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${D435_TOPIC}" || return 1
|
||||
timeout 3 ros2 topic echo --once "${D435_TOPIC}" 2>/dev/null | grep -q "^header:"
|
||||
}
|
||||
if _d435_ok; then
|
||||
echo "=== 顶视 ${D435_TOPIC} 已在发图，跳过拉起 ==="
|
||||
else
|
||||
echo "=== 拉起顶视 D435（一阶段一次性定位）==="
|
||||
D435_ARGS=()
|
||||
[[ "${CAMERA_PREVIEW}" == true ]] && D435_ARGS+=(--preview) || D435_ARGS+=(--no-preview)
|
||||
nohup bash "${ROOT}/scripts/run_camera_d435.sh" \
|
||||
"${D435_ARGS[@]}" > /tmp/run_all_d435.log 2>&1 &
|
||||
echo " d435 pid=$! log=/tmp/run_all_d435.log"
|
||||
d435_ok=false
|
||||
for _i in $(seq 1 24); do
|
||||
if _d435_ok; then d435_ok=true; break; fi
|
||||
sleep 0.5
|
||||
done
|
||||
if [[ "${d435_ok}" != true ]]; then
|
||||
echo "ERROR: ${D435_TOPIC} 仍无图像。看日志：/tmp/run_all_d435.log" >&2
|
||||
echo " 混合跟踪没有 D435 就切不出去。USB 独占或没插好都会这样。" >&2
|
||||
exit 1
|
||||
fi
|
||||
echo " 顶视 D435 出图 OK"
|
||||
fi
|
||||
else
|
||||
echo "=== --no-camera：跳过相机检查 ==="
|
||||
fi
|
||||
|
||||
# ---- 3) 触觉服务。只有左臂装了传感器；只有三阶段搜孔才用力 ----
|
||||
# --skip-stage3 时如果还去等，USB 打不开会假死：话题在、connected 却是 false，
|
||||
# 每次 echo 吃满超时 × 60 次 ≈ 3 分钟，终端上没有任何新行（2026-10-09 真机）。
|
||||
_SKIP_TACTILE_REASON=""
|
||||
if [[ "${SKIP_STAGE3}" == true && "${WITH_TACTILE}" == true ]]; then
|
||||
echo "=== 搜孔已取消：跟踪和对孔不用触觉，跳过拉起 ==="
|
||||
WITH_TACTILE=false
|
||||
_SKIP_TACTILE_REASON="stage3"
|
||||
fi
|
||||
|
||||
# 同样先查 topic 列表再 echo，避免服务没起时反复撞超时。
|
||||
TACTILE_TOPIC="/tactile/left/status"
|
||||
_tactile_ok() {
|
||||
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${TACTILE_TOPIC}" || return 1
|
||||
timeout 1 ros2 topic echo "${TACTILE_TOPIC}" --once 2>/dev/null | grep -q "connected: true"
|
||||
}
|
||||
|
||||
if [[ "${WITH_TACTILE}" == true ]]; then
|
||||
# 本臂有没有触觉由 site.yaml 的 robots[].tactile 说了算。
|
||||
# 以前这里写的是 `SN == Rizon4-063989`：换一台机器人就再也拉不起触觉服务，
|
||||
# 而且失败现象是"搜孔时力阈值全是 0"，离真正原因很远。
|
||||
if [[ "${SITE_ROBOT_HAS_TACTILE:-0}" == "1" ]]; then
|
||||
if _tactile_ok; then
|
||||
echo "=== 触觉 ${TACTILE_TOPIC} 已在发，跳过拉起 ==="
|
||||
else
|
||||
echo "=== 拉起触觉服务（左臂）==="
|
||||
# 先清干净：上一次异常退出留下的 worker 会占死 /dev/video*，
|
||||
# 新进程会以"打不开相机"的形式失败，报错信息看不出根因。
|
||||
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
|
||||
pkill orisys_tac_w 2>/dev/null || true
|
||||
pkill -f 'arm_application/tactile_node.py' 2>/dev/null || true
|
||||
sleep 1.0
|
||||
nohup ros2 launch arm_application tactile.launch.py > /tmp/run_all_tactile.log 2>&1 &
|
||||
echo " tactile pid=$! log=/tmp/run_all_tactile.log"
|
||||
echo " 等触觉 connected（USB 打不开会在十几秒内跳过，不会空等几分钟）"
|
||||
t_ok=false
|
||||
for _i in $(seq 1 8); do
|
||||
if _tactile_ok; then
|
||||
t_ok=true
|
||||
break
|
||||
fi
|
||||
# 日志已经写明打不开：再给两轮自愈，然后放行。不许在这里空转到超时。
|
||||
if [[ "${_i}" -ge 4 ]] && grep -q "connect failed" /tmp/run_all_tactile.log 2>/dev/null; then
|
||||
echo " 触觉 USB 打不开，停止等待" >&2
|
||||
break
|
||||
fi
|
||||
echo " 触觉还未 connected（${_i}/8）"
|
||||
sleep 0.5
|
||||
done
|
||||
if [[ "${t_ok}" != true ]]; then
|
||||
# 触觉不通不阻断主流程：三阶段的搜孔轻压需要力数据，但一/二阶段不需要。
|
||||
# 让用户自己判断是先把传感器弄好，还是只跑到对孔就停。
|
||||
echo "WARN: 触觉话题仍未就绪（传感器没接？）。流程继续，三阶段搜孔会受影响。" >&2
|
||||
echo " 日志：/tmp/run_all_tactile.log" >&2
|
||||
else
|
||||
echo " 触觉就绪 OK"
|
||||
fi
|
||||
fi
|
||||
else
|
||||
echo "=== 触觉只有左臂装了；当前 ${SITE_ROBOT_ARM:-?}，跳过 ==="
|
||||
fi
|
||||
elif [[ "${_SKIP_TACTILE_REASON}" != "stage3" ]]; then
|
||||
echo "=== --no-tactile：跳过触觉检查 ==="
|
||||
fi
|
||||
|
||||
# ---- 4) 清掉可能打架的旧进程 ----
|
||||
echo "=== 清理旧的一阶段伺服/感知进程 ==="
|
||||
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
|
||||
pkill -f 'lib/arm_application/assembly_pipeline' 2>/dev/null || true
|
||||
sleep 1
|
||||
|
||||
# ---- 5) exec pipeline.launch.py（把 CLI 映射成它的 launch 参数）----
|
||||
ARGS=(
|
||||
"skip_stage1:=${SKIP_STAGE1}"
|
||||
"skip_stage2:=${SKIP_STAGE2}"
|
||||
"skip_stage3:=${SKIP_STAGE3}"
|
||||
"preview:=${PIPELINE_PREVIEW}"
|
||||
"resnap:=${RESNAP}"
|
||||
"no_descent:=${NO_DESCENT}"
|
||||
"d435_move_only:=false"
|
||||
)
|
||||
[[ -n "${INIT_Z}" ]] && ARGS+=("init_z_m:=${INIT_Z}")
|
||||
[[ -n "${HOVER_Z}" ]] && ARGS+=("hover_z_m:=${HOVER_Z}")
|
||||
[[ -n "${FINAL_GAP}" ]] && ARGS+=("final_gap_mm:=${FINAL_GAP}")
|
||||
[[ -n "${HOLE}" ]] && ARGS+=("target_hole:=${HOLE}")
|
||||
[[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}")
|
||||
[[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}")
|
||||
|
||||
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} init_z=${INIT_Z:-site} hover_z=${HOVER_Z:-site} final_gap=${FINAL_GAP:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} ==="
|
||||
exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}"
|
||||
Executable
+166
@@ -0,0 +1,166 @@
|
||||
#!/usr/bin/env bash
|
||||
# 终端方式运行 D405：走官方 pyrealsense2 SDK —— 发布彩色 + 深度 + 双目 IR 话题。
|
||||
# 带 --preview 时，Qt 窗口跑在**独立的**查看器进程（`realsense_view`）里，
|
||||
# 这样绘制界面永远不会拖慢相机数据流。关掉窗口 ≠ 停止相机。
|
||||
# 不要同时启动 `view_realsense_depth.sh`（USB 是独占的）。
|
||||
#
|
||||
# 用法：
|
||||
# bash ~/ros2/scripts/run_camera_d405.sh
|
||||
# bash ~/ros2/scripts/run_camera_d405.sh --preview
|
||||
# bash ~/ros2/scripts/run_camera_d405.sh --arm 91 --preview
|
||||
|
||||
set -eo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
PREVIEW=-1 # -1 = 沿用 site；0/1 = 强制
|
||||
HIGH_FPS=0
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||||
parse_arm_args "$@"
|
||||
set -- "${PARSE_ARM_REST[@]}"
|
||||
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--preview) PREVIEW=1; shift ;;
|
||||
--no-preview) PREVIEW=0; shift ;;
|
||||
--30fps) HIGH_FPS=1; shift ;;
|
||||
-h|--help) sed -n '2,14p' "$0"; exit 0 ;;
|
||||
*) echo "Unknown: $1" >&2; exit 1 ;;
|
||||
esac
|
||||
done
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
# 用源码里的采集逻辑，不走 install 里那份旧的。install 的入口仍由 ros2 run 找到。
|
||||
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
|
||||
CAM_NAME="${SITE_CAM_NAME:-d405}"
|
||||
# 只杀本臂这台腕部相机。以前 pkill 全部 realsense_color_pub / realsense_view，
|
||||
# 会把已经开着的顶视 D435 一起干掉（或反过来：后开 D435 把 D405 预览窗杀掉），
|
||||
# 现场看起来就像"只剩眼外画面、绿框也不见了"。
|
||||
pkill -f "camera_name:=${CAM_NAME}( |$)" 2>/dev/null || true
|
||||
sleep 2
|
||||
|
||||
USB_INFO="$(python3 - <<PY 2>/dev/null || true
|
||||
import pyrealsense2 as rs
|
||||
want_sn = "${SITE_CAM_SERIAL:-}"
|
||||
ds = list(rs.context().devices)
|
||||
if not ds:
|
||||
print("")
|
||||
raise SystemExit
|
||||
pick = None
|
||||
for d in ds:
|
||||
sn = d.get_info(rs.camera_info.serial_number)
|
||||
if want_sn and sn == want_sn:
|
||||
pick = d
|
||||
break
|
||||
if pick is None:
|
||||
for d in ds:
|
||||
name = (d.get_info(rs.camera_info.name) or "").lower()
|
||||
if "405" in name:
|
||||
pick = d
|
||||
break
|
||||
pick = pick or ds[0]
|
||||
usb = pick.get_info(rs.camera_info.usb_type_descriptor)
|
||||
sn = pick.get_info(rs.camera_info.serial_number)
|
||||
name = pick.get_info(rs.camera_info.name)
|
||||
print(f"{usb}|{sn}|{name}")
|
||||
PY
|
||||
)"
|
||||
USB_TYPE="${USB_INFO%%|*}"
|
||||
echo "=== RealSense USB=${USB_TYPE:-unknown} ${USB_INFO} ==="
|
||||
echo "=== site: ${SITE_CONFIG:-?} weights=${SITE_YOLO_WEIGHTS:-?} ==="
|
||||
|
||||
if [[ "${USB_TYPE}" == 2* ]]; then
|
||||
CAM_W=640; CAM_H=480; CAM_FPS=15
|
||||
echo "=== camera 640x480@15 (USB2: depth+IR still ON) ==="
|
||||
echo "=== WARN: D405 is on USB2. Plug it into a USB3 (blue) port for 848x480@30 ==="
|
||||
elif [[ "${SITE_CAM_USE_SITE_RES:-1}" == "1" ]]; then
|
||||
# site.yaml camera.width/height/fps 优先；--30fps 只抬帧率，不再写死 848x480
|
||||
CAM_W="${SITE_CAM_WIDTH:-848}"
|
||||
CAM_H="${SITE_CAM_HEIGHT:-480}"
|
||||
CAM_FPS="${SITE_CAM_FPS:-30}"
|
||||
if [[ "${HIGH_FPS}" -eq 1 && "${CAM_FPS}" -lt 30 ]]; then
|
||||
CAM_FPS=30
|
||||
fi
|
||||
echo "=== camera ${CAM_W}x${CAM_H}@${CAM_FPS} serial=${SITE_CAM_SERIAL:-} arm=${SITE_ROBOT_SN:-} ==="
|
||||
elif [[ "${HIGH_FPS}" -eq 1 ]]; then
|
||||
CAM_W=848; CAM_H=480; CAM_FPS=30
|
||||
echo "=== camera 848x480@30 (--30fps, site use_site_resolution=false) ==="
|
||||
else
|
||||
CAM_W=1280; CAM_H=720; CAM_FPS=15
|
||||
echo "=== camera 1280x720@15 ==="
|
||||
fi
|
||||
|
||||
CAM_ARGS=(
|
||||
--ros-args
|
||||
-p camera_name:="${CAM_NAME}"
|
||||
-p width:="${CAM_W}"
|
||||
-p height:="${CAM_H}"
|
||||
-p fps:="${CAM_FPS}"
|
||||
)
|
||||
if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then
|
||||
CAM_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}")
|
||||
fi
|
||||
CAM_ARGS+=(
|
||||
-p auto_exposure:="$([ "${SITE_CAM_AUTO_EXPOSURE:-1}" = "1" ] && echo true || echo false)"
|
||||
-p ae_limit_us:="${SITE_CAM_AE_LIMIT_US:-4000}"
|
||||
-p emitter_enabled:="$([ "${SITE_CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
|
||||
-p laser_power:="${SITE_CAM_LASER_POWER:-0}"
|
||||
)
|
||||
if [[ -n "${SITE_CAM_EXPOSURE_US:-}" && "${SITE_CAM_EXPOSURE_US}" != "0" && "${SITE_CAM_EXPOSURE_US}" != "0.0" ]]; then
|
||||
CAM_ARGS+=(-p exposure_us:="${SITE_CAM_EXPOSURE_US}")
|
||||
fi
|
||||
if [[ -n "${SITE_CAM_GAIN:-}" && "${SITE_CAM_GAIN}" != "0" && "${SITE_CAM_GAIN}" != "0.0" ]]; then
|
||||
CAM_ARGS+=(-p gain:="${SITE_CAM_GAIN}")
|
||||
fi
|
||||
|
||||
if [[ "${SITE_CAM_ENABLE_DEPTH:-1}" == "1" ]]; then
|
||||
CAM_ARGS+=(-p enable_depth:=true)
|
||||
echo "=== depth ON ==="
|
||||
else
|
||||
CAM_ARGS+=(-p enable_depth:=false)
|
||||
echo "=== depth OFF (site.yaml) ==="
|
||||
fi
|
||||
|
||||
DO_PREVIEW="${SITE_CAM_PREVIEW:-1}"
|
||||
if [[ "${PREVIEW}" -ge 0 ]]; then
|
||||
DO_PREVIEW="${PREVIEW}"
|
||||
fi
|
||||
# 程序界面只看彩色和深度。红外是旧的四窗格查看器才要的。
|
||||
# 彩色要等红外齐帧时，场景一动，整组帧对不齐，彩色就被丢掉。
|
||||
if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then
|
||||
CAM_ARGS+=(-p enable_infra:=true)
|
||||
else
|
||||
CAM_ARGS+=(-p enable_infra:=false)
|
||||
fi
|
||||
if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_PREVIEW_STEREO:-1}" == "1" ]]; then
|
||||
CAM_ARGS+=(-p preview_stereo:=true)
|
||||
else
|
||||
CAM_ARGS+=(-p preview_stereo:=false)
|
||||
fi
|
||||
|
||||
if [[ "${DO_PREVIEW}" == "1" ]]; then
|
||||
echo "=== SDK preview ON (separate viewer process, hold ${SITE_CAM_OVERLAY_HOLD:-0.1}s) ==="
|
||||
echo "=== close window = hide preview only; camera keeps publishing ==="
|
||||
fi
|
||||
|
||||
echo "=== 本脚本只出图，不跑 YOLO。绿框来自 perception_node，请另开终端： ==="
|
||||
echo "=== bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} ==="
|
||||
|
||||
if [[ "${DO_PREVIEW}" == "1" ]]; then
|
||||
# 相机进程只负责发布；Qt 查看器跑在自己的进程里，所以界面绘制
|
||||
# 永远不会把采集循环饿死（以前共用 GIL 时每帧要花约 70 ms）。
|
||||
# 前面已经按 camera_name 杀过本臂进程，这里不要再杀全部 realsense_view。
|
||||
ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
|
||||
CAM_PID=$!
|
||||
trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
|
||||
ros2 run arm_application realsense_view --ros-args \
|
||||
-p camera_name:="${CAM_NAME}" \
|
||||
-p overlay_hold_sec:="${SITE_CAM_OVERLAY_HOLD:-0.1}" \
|
||||
-p hud_label:="${SITE_ROBOT_LABEL:-} ${SITE_ROBOT_SN:-} ${SITE_CAM_SERIAL:-}"
|
||||
else
|
||||
exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
|
||||
fi
|
||||
Executable
+60
@@ -0,0 +1,60 @@
|
||||
#!/usr/bin/env bash
|
||||
# 两臂之间的顶置 D435（`eye-to-hand`，相机固定在外）。
|
||||
# 可以和 run_camera_d405.sh 同时开：只杀 camera_name:=d435，不动腕部那台。
|
||||
#
|
||||
# bash ~/ros2/scripts/run_camera_d435.sh --preview
|
||||
#
|
||||
set -eo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
PREVIEW=1
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--preview) PREVIEW=1; shift ;;
|
||||
--no-preview) PREVIEW=0; shift ;;
|
||||
-h|--help) sed -n '2,8p' "$0"; exit 0 ;;
|
||||
*) echo "Unknown: $1" >&2; exit 1 ;;
|
||||
esac
|
||||
done
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
# 用源码里的采集逻辑，不走 install 里那份旧的。install 的入口仍由 ros2 run 找到。
|
||||
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
SN="${SITE_D435_SERIAL:-}"
|
||||
if [[ -z "${SN}" ]]; then
|
||||
echo "ERROR: cameras.d435.serial empty in site.yaml" >&2
|
||||
exit 2
|
||||
fi
|
||||
echo "=== D435 overhead serial=${SN} ${SITE_D435_WIDTH:-1280}x${SITE_D435_HEIGHT:-720}@${SITE_D435_FPS:-30} ==="
|
||||
# 只杀顶视这一台。不要 pkill 全部 realsense_view —— 那会把正在看的 D405 预览窗干掉，
|
||||
# 绿框叠在 D405 窗口上，窗没了就像"伺服没画面、也没工件框"。
|
||||
pkill -f "camera_name:=d435( |$)" 2>/dev/null || true
|
||||
CAM_ARGS=(
|
||||
--ros-args
|
||||
-p camera_name:=d435
|
||||
-p camera_serial:="${SN}"
|
||||
-p width:="${SITE_D435_WIDTH:-1280}"
|
||||
-p height:="${SITE_D435_HEIGHT:-720}"
|
||||
-p fps:="${SITE_D435_FPS:-30}"
|
||||
-p enable_depth:=true
|
||||
-p enable_infra:=false
|
||||
-p preview_stereo:=false
|
||||
-p auto_exposure:=true
|
||||
-p ae_limit_us:=8000
|
||||
-p emitter_enabled:=true
|
||||
)
|
||||
if [[ "${PREVIEW}" == "1" ]]; then
|
||||
ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
|
||||
CAM_PID=$!
|
||||
trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
|
||||
# 绿框来自 workpiece/detection_overhead（D435 像素）。不要订 workpiece/detection。
|
||||
ros2 run arm_application realsense_view --ros-args \
|
||||
-p camera_name:=d435 \
|
||||
-p overlay_detection:=true \
|
||||
-p detection_topic:=workpiece/detection_overhead \
|
||||
-p overlay_hold_sec:=1.5 \
|
||||
-p hud_label:="D435 顶视"
|
||||
else
|
||||
exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
|
||||
fi
|
||||
Executable
+136
@@ -0,0 +1,136 @@
|
||||
#!/usr/bin/env bash
|
||||
# 真机 Rizon4 启动入口 —— 设置 RDK 运行时库，然后以 backend:=real 启动整栈
|
||||
# 注意：这里不加 `set -u` —— conda/RoboStack 的钩子会引用未绑定的变量（CONDA_BUILD、COLCON_TRACE）
|
||||
#
|
||||
# bash ~/ros2/scripts/run_real.sh
|
||||
# bash ~/ros2/scripts/run_real.sh --arm 91
|
||||
# bash ~/ros2/scripts/run_real.sh --sn Rizon4-063991 # 本次启动换臂（不改 yaml）
|
||||
set -eo pipefail
|
||||
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
FLEXIV_PREFIX="${HOME}/flexiv_install"
|
||||
LIST_ONLY=false
|
||||
NO_STOP=false
|
||||
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--sn|--arm) export ROBOT_SN="${2:?}"; export ROBOT_ARM="${2}"; shift 2 ;;
|
||||
--no-stop) NO_STOP=true; shift ;;
|
||||
--list|-l) LIST_ONLY=true; shift ;;
|
||||
-h|--help)
|
||||
sed -n '2,8p' "$0"
|
||||
echo " 机械臂模型在工位程序里看，这里默认不开 RViz。"
|
||||
echo " 其余参数传给 ros2 launch。要临时打开 RViz：use_rviz:=true"
|
||||
exit 0
|
||||
;;
|
||||
*) break ;;
|
||||
esac
|
||||
done
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/dev_tools.sh"
|
||||
dev_source_conda
|
||||
conda activate ros_flexiv
|
||||
if [[ ! -f "${ROOT}/install/setup.bash" ]]; then
|
||||
echo "ERROR: ${ROOT}/install/setup.bash 不存在。先在仓库根目录 colcon build，不要在中文路径下编。" >&2
|
||||
exit 1
|
||||
fi
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/install/setup.bash"
|
||||
# 本机同时有 /opt/ros/humble。install 会把它排到 conda 前面，
|
||||
# 3.10 的 ros2 会去加载 ros_flexiv 里 3.11 的 rclpy。把 conda 的 ros2 放回最前。
|
||||
if [[ -n "${CONDA_PREFIX:-}" && -x "${CONDA_PREFIX}/bin/ros2" ]]; then
|
||||
export PATH="${CONDA_PREFIX}/bin:${PATH}"
|
||||
fi
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
# 真机与仿真的 DDS 域隔离：这个入口**不经过** env.sh，所以这里也得纠正一次。
|
||||
# shellcheck source=lib/ros_domain.sh
|
||||
source "${ROOT}/scripts/lib/ros_domain.sh"
|
||||
ros_domain_force_real
|
||||
|
||||
# `flexiv_hardware` 必须从 flexiv_install 加载 spdlog/RBDyn（不能用 conda 的 fmt v11）
|
||||
export LD_LIBRARY_PATH="${FLEXIV_PREFIX}/lib:${LD_LIBRARY_PATH:-}"
|
||||
|
||||
# `SITE_ROBOT_SN` 是解析后的 Flexiv 序列号（89/91 别名已经映射好了）。
|
||||
ROBOT_SN="${SITE_ROBOT_SN:?run_real.sh 需要已配置的机械臂 SN —— 见 scripts/lib/load_site_config.sh 的报错}"
|
||||
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
|
||||
NET_IFACE="${SITE_NET_IFACE:-}"
|
||||
RDK_PYTHON="$(dev_rdk_python)"
|
||||
|
||||
if [[ "${LIST_ONLY}" == true ]]; then
|
||||
python3 "${ROOT}/scripts/lib/export_site_env.py" --print
|
||||
exit 0
|
||||
fi
|
||||
|
||||
echo "=== site: ${SITE_CONFIG:-n/a} SN=${ROBOT_SN} ${SITE_ROBOT_LABEL:-} arm=${SITE_ROBOT_ARM:-} ==="
|
||||
echo "=== catalog: ${SITE_ROBOT_CATALOG:-} ==="
|
||||
echo "=== handeye: ${SITE_HANDEYE_FILE:-} ==="
|
||||
echo "=== camera serial: ${SITE_CAM_SERIAL:-} gripper: ${SITE_GRIP_MODEL:-} ${SITE_GRIP_PORT:-} ==="
|
||||
echo "=== network iface=${NET_IFACE:-auto} robot=${ROBOT_IP} pc=${SITE_PC_IP:-} ==="
|
||||
if [[ "${NO_STOP}" == true ]]; then
|
||||
echo "=== --no-stop：不扫 install/（感知和伺服还在，扫了会把自己杀掉）==="
|
||||
else
|
||||
echo "=== stop leftover real stack (RDK exclusive) ==="
|
||||
# 以前这里是两行 pkill（ros2_control_node + stack.launch），漏掉了
|
||||
# arm_control 那一族**独立存活**的孤儿（adapter / named_pose_server /
|
||||
# arm_status_node / joint_state_publisher）—— 它们会冒名顶替新一轮的栈，
|
||||
# 见 lib/real_procs.sh 的文件头。扫描规则与 stop_real.sh 共用一份。
|
||||
# shellcheck source=lib/real_procs.sh
|
||||
source "${ROOT}/scripts/lib/real_procs.sh"
|
||||
mapfile -t _stale < <(real_proc_targets)
|
||||
if [ "${#_stale[@]}" != "0" ]; then
|
||||
echo "清掉上一轮残留（${#_stale[@]} 个）："
|
||||
for _pid in "${_stale[@]}"; do
|
||||
printf ' %-7s %s\n' "$_pid" "$(ps -o args= -p "$_pid" 2>/dev/null | cut -c1-120)"
|
||||
kill -TERM "$_pid" 2>/dev/null || true
|
||||
done
|
||||
sleep 1
|
||||
for _pid in "${_stale[@]}"; do
|
||||
kill -0 "$_pid" 2>/dev/null && kill -KILL "$_pid" 2>/dev/null || true
|
||||
done
|
||||
unset _pid
|
||||
fi
|
||||
unset _stale
|
||||
fi
|
||||
sleep 1
|
||||
# 清掉把 91 的 .100 误绑到 89 MAC 上的陈旧 ARP
|
||||
if [[ -n "${NET_IFACE}" ]]; then
|
||||
ip neigh del "${ROBOT_IP}" dev "${NET_IFACE}" 2>/dev/null || true
|
||||
fi
|
||||
if ping -c 2 -W 1 "${ROBOT_IP}" >/dev/null 2>&1; then
|
||||
echo "robot ${ROBOT_IP} ping OK"
|
||||
else
|
||||
echo "WARN: cannot ping ${ROBOT_IP} — 这一台不在网里，RDK 大概率会失败。"
|
||||
echo " 本机: $(ip -4 -o addr show dev "${NET_IFACE:-enp4s0}" 2>/dev/null | awk '{print $4}' | tr '\n' ' ')"
|
||||
echo " ARP ${ROBOT_IP}: $(ip neigh show "${ROBOT_IP}" 2>/dev/null | head -1 || echo none)"
|
||||
MAC91="$(ip neigh show "${ROBOT_IP}" 2>/dev/null | awk '{print $5}' | head -1)"
|
||||
MAC89="$(ip neigh show 192.168.2.101 2>/dev/null | awk '{print $5}' | head -1)"
|
||||
if [[ -n "${MAC91}" && -n "${MAC89}" && "${MAC91}" == "${MAC89}" ]]; then
|
||||
echo " ARP MAC 与 89(192.168.2.101) 相同 → .100 是误绑，91 并没有回包。"
|
||||
fi
|
||||
echo " 91 请接控制柜 User Port 1（出厂静态 192.168.2.100）到同一交换机，示教器 AUTO+Remote Ethernet。"
|
||||
echo " 89=192.168.2.101 91=192.168.2.100"
|
||||
fi
|
||||
|
||||
echo "=== RDK preflight + tool (${ROBOT_SN}) ==="
|
||||
TOOL_NAME="${SITE_ROBOT_TOOL_NAME:-}"
|
||||
RDK_TOOL_ARGS=(--sn "${ROBOT_SN}")
|
||||
if [[ -n "${NET_IFACE}" ]]; then
|
||||
RDK_TOOL_ARGS+=(--iface "${NET_IFACE}")
|
||||
fi
|
||||
if [[ -n "${TOOL_NAME}" ]]; then
|
||||
RDK_TOOL_ARGS+=(--name "${TOOL_NAME}")
|
||||
echo "=== Studio tool → ${TOOL_NAME} ==="
|
||||
fi
|
||||
if ! "${RDK_PYTHON}" "${ROOT}/scripts/rdk_switch_tool.py" "${RDK_TOOL_ARGS[@]}"; then
|
||||
echo ""
|
||||
echo "ERROR: RDK cannot connect / switch tool on ${ROBOT_SN}."
|
||||
echo " 先确认 ping ${ROBOT_IP} 通（91=控制柜 User Port 1 → 192.168.2.100）。"
|
||||
echo " Stop any other RDK client (this run_real.sh is exclusive)."
|
||||
echo " On teach pendant: Settings → Remote → enable Ethernet/RDK remote mode."
|
||||
echo " Test: bash ${ROOT}/scripts/test_rdk_python.sh ${ROBOT_SN}"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
exec ros2 launch arm_bringup stack.launch.py backend:=real robot_sn:="${ROBOT_SN}" use_rviz:=false "$@"
|
||||
@@ -0,0 +1 @@
|
||||
"""arm_application：从旧工位抽出的节点。"""
|
||||
@@ -0,0 +1,85 @@
|
||||
#!/usr/bin/env python3
|
||||
"""针对 YOLO ``xyxy`` 框（像素坐标系）的匀速跟踪器。
|
||||
|
||||
用于避免工件快速运动时丢失锁定：下一帧的闸门使用*预测*框，
|
||||
短暂的 YOLO 漏检则由惯性滑行（coast）维持叠加显示。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _center(xyxy: np.ndarray) -> np.ndarray:
|
||||
return np.array(
|
||||
[0.5 * (float(xyxy[0]) + float(xyxy[2])), 0.5 * (float(xyxy[1]) + float(xyxy[3]))],
|
||||
dtype=float,
|
||||
)
|
||||
|
||||
|
||||
def _wh(xyxy: np.ndarray) -> Tuple[float, float]:
|
||||
return (
|
||||
max(8.0, float(xyxy[2]) - float(xyxy[0])),
|
||||
max(8.0, float(xyxy[3]) - float(xyxy[1])),
|
||||
)
|
||||
|
||||
|
||||
class BoxTracker:
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
vmax_px_s: float = 1800.0,
|
||||
v_lpf: float = 0.70,
|
||||
max_dt: float = 0.25,
|
||||
) -> None:
|
||||
self.vmax_px_s = float(vmax_px_s)
|
||||
self.v_lpf = float(np.clip(v_lpf, 0.05, 1.0))
|
||||
self.max_dt = float(max_dt)
|
||||
self.xyxy: Optional[np.ndarray] = None
|
||||
self.v = np.zeros(2, dtype=float) # 中心点 px/s
|
||||
self.t: Optional[float] = None
|
||||
|
||||
def reset(self) -> None:
|
||||
self.xyxy = None
|
||||
self.v[:] = 0.0
|
||||
self.t = None
|
||||
|
||||
def predict(self, t: float, img_wh: Optional[Tuple[int, int]] = None) -> Optional[np.ndarray]:
|
||||
if self.xyxy is None:
|
||||
return None
|
||||
out = self.xyxy.copy()
|
||||
if self.t is None:
|
||||
return out
|
||||
dt = float(np.clip(t - self.t, 0.0, self.max_dt))
|
||||
if dt <= 1e-4:
|
||||
return out
|
||||
c = _center(out) + self.v * dt
|
||||
w, h = _wh(out)
|
||||
x1, y1 = c[0] - 0.5 * w, c[1] - 0.5 * h
|
||||
x2, y2 = c[0] + 0.5 * w, c[1] + 0.5 * h
|
||||
if img_wh is not None:
|
||||
iw, ih = float(img_wh[0]), float(img_wh[1])
|
||||
x1 = float(np.clip(x1, 0.0, iw - 2.0))
|
||||
y1 = float(np.clip(y1, 0.0, ih - 2.0))
|
||||
x2 = float(np.clip(x2, x1 + 4.0, iw))
|
||||
y2 = float(np.clip(y2, y1 + 4.0, ih))
|
||||
return np.array([x1, y1, x2, y2], dtype=float)
|
||||
|
||||
def update(self, xyxy: np.ndarray, t: float) -> None:
|
||||
xyxy = np.asarray(xyxy, dtype=float).reshape(4)
|
||||
c = _center(xyxy)
|
||||
if self.xyxy is not None and self.t is not None:
|
||||
dt = float(t - self.t)
|
||||
if dt > 0.28:
|
||||
self.v[:] = 0.0
|
||||
elif dt > 1e-3:
|
||||
inst = (c - _center(self.xyxy)) / dt
|
||||
n = float(np.hypot(inst[0], inst[1]))
|
||||
if n > self.vmax_px_s > 0.0:
|
||||
inst = inst * (self.vmax_px_s / n)
|
||||
a = self.v_lpf
|
||||
self.v = a * inst + (1.0 - a) * self.v
|
||||
self.xyxy = xyxy.copy()
|
||||
self.t = float(t)
|
||||
@@ -0,0 +1,725 @@
|
||||
#!/usr/bin/env python3
|
||||
"""DH Robotics PGE / PGEA Modbus RTU 客户端。
|
||||
|
||||
移植自官方 C++ 驱动：
|
||||
DH-Robotics/dh_gripper_ros → DH_Modbus_Gripper
|
||||
（工厂会按型号名里是否包含 ``PGE`` / ``PGEA`` 映射到本类）
|
||||
|
||||
Flexiv RDK 的 ``Gripper`` 驱动不了这个夹爪：它不是 Studio Device。
|
||||
改用 RS485（USB–UART）通信，寄存器与 DH 的 ROS 驱动完全一致。
|
||||
|
||||
寄存器（保持寄存器，FC03 读 / FC06 写，CRC-16/Modbus 小端）：
|
||||
0x0100 初始化，写 0xA5
|
||||
0x0101 力 20–100 %
|
||||
0x0103 位置 0–1000 ‰
|
||||
0x0104 速度 1–100 %
|
||||
0x0200 初始化状态 0=需要 1=完成 2=进行中
|
||||
0x0201 夹持状态 0=运动中 1=已到位 2=已夹住 3=已掉落
|
||||
0x0202 当前位置
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
try:
|
||||
import serial
|
||||
except ImportError as exc: # pragma: no cover
|
||||
raise SystemExit(
|
||||
"缺少 pyserial。在 conda 环境 ros_flexiv 里执行：pip install pyserial"
|
||||
) from exc
|
||||
|
||||
|
||||
# 官方 CRC-16/Modbus（与 dh_modbus_gripper.cpp 使用同一张表）。
|
||||
_CRC_TABLE = (
|
||||
0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
|
||||
0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
|
||||
0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
|
||||
0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
|
||||
0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
|
||||
0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
|
||||
0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
|
||||
0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
|
||||
0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
|
||||
0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
|
||||
0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
|
||||
0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
|
||||
0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
|
||||
0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
|
||||
0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
|
||||
0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
|
||||
0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
|
||||
0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
|
||||
0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
|
||||
0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
|
||||
0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
|
||||
0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
|
||||
0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
|
||||
0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
|
||||
0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
|
||||
0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
|
||||
0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
|
||||
0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
|
||||
0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
|
||||
0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
|
||||
0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
|
||||
0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040,
|
||||
)
|
||||
|
||||
REG_INIT = 0x0100
|
||||
REG_FORCE = 0x0101
|
||||
REG_POSITION = 0x0103
|
||||
REG_SPEED = 0x0104
|
||||
REG_INIT_STATE = 0x0200
|
||||
REG_GRIP_STATE = 0x0201
|
||||
REG_CUR_POSITION = 0x0202
|
||||
|
||||
INIT_CMD = 0xA5
|
||||
|
||||
S_INIT_NOT = 0
|
||||
S_INIT_FINISHED = 1
|
||||
S_INIT_DOING = 2
|
||||
|
||||
S_GRIP_MOVING = 0
|
||||
S_GRIP_ARRIVED = 1
|
||||
S_GRIP_CAUGHT = 2
|
||||
S_GRIP_DROPPED = 3
|
||||
|
||||
GRIP_STATE_NAME = {
|
||||
S_GRIP_MOVING: "moving",
|
||||
S_GRIP_ARRIVED: "arrived",
|
||||
S_GRIP_CAUGHT: "caught",
|
||||
S_GRIP_DROPPED: "dropped",
|
||||
}
|
||||
|
||||
INIT_STATE_NAME = {
|
||||
S_INIT_NOT: "need_init",
|
||||
S_INIT_FINISHED: "initialized",
|
||||
S_INIT_DOING: "initializing",
|
||||
}
|
||||
|
||||
|
||||
def latched_position_break(command: int, current: int, target: int) -> Optional[int]:
|
||||
"""命令寄存器已经等于目标、手指却不在目标上时，返回要先写入的中间位置。
|
||||
|
||||
大寰固件：再写一遍与 0x0103 当前值相同的数，不会启动新运动。
|
||||
开爪卡在 caught 时就是这样——命令已是 1000，手指停在夹持位。
|
||||
先把命令改成当前位置（手指本来就在这，不会再夹紧），等读回确认
|
||||
固件已经采样，再写最终目标。只 sleep 几十毫秒就写回旧值，控制周期
|
||||
来不及看到中间值，最终仍是旧命令，手指不动。
|
||||
"""
|
||||
cmd = int(max(0, min(1000, command)))
|
||||
cur = int(max(0, min(1000, current)))
|
||||
pos = int(max(0, min(1000, target)))
|
||||
if cmd != pos or abs(cur - pos) <= 25:
|
||||
return None
|
||||
if cur != cmd:
|
||||
return cur
|
||||
return 0 if cmd != 0 else 40
|
||||
|
||||
|
||||
def grip_motion_settled(
|
||||
grip_state: int, position: int, target: int, seen_motion: bool
|
||||
) -> bool:
|
||||
"""这一拍能不能算「这句位置命令已经结束」。
|
||||
|
||||
张开前本来就是 caught、位置还停在夹持位：不能当成张开完成。
|
||||
闭合夹住物体：动过之后停在比目标更开的位置，算完成。
|
||||
"""
|
||||
near = abs(int(position) - int(target)) <= 25
|
||||
if grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and near:
|
||||
return True
|
||||
if grip_state == S_GRIP_CAUGHT and seen_motion:
|
||||
if int(target) + 25 < int(position):
|
||||
return True
|
||||
if near:
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def crc16_modbus(data: bytes) -> int:
|
||||
"""官方 DH CRC16（Modbus，线路上按小端发送）。"""
|
||||
crc = 0xFFFF
|
||||
for byte in data:
|
||||
crc = (crc >> 8) ^ _CRC_TABLE[(crc ^ byte) & 0xFF]
|
||||
return crc
|
||||
|
||||
|
||||
def _frame(slave: int, func: int, index: int, value: int) -> bytes:
|
||||
body = bytes(
|
||||
[
|
||||
slave & 0xFF,
|
||||
func & 0xFF,
|
||||
(index >> 8) & 0xFF,
|
||||
index & 0xFF,
|
||||
(value >> 8) & 0xFF,
|
||||
value & 0xFF,
|
||||
]
|
||||
)
|
||||
crc = crc16_modbus(body)
|
||||
return body + bytes([crc & 0xFF, (crc >> 8) & 0xFF])
|
||||
|
||||
|
||||
@dataclass
|
||||
class GripperState:
|
||||
init_state: int = S_INIT_NOT
|
||||
grip_state: int = S_GRIP_MOVING
|
||||
position: int = 0
|
||||
target_position: int = 0
|
||||
target_force: int = 0
|
||||
stroke_mm: float = 20.0
|
||||
|
||||
@property
|
||||
def initialized(self) -> bool:
|
||||
return self.init_state == S_INIT_FINISHED
|
||||
|
||||
@property
|
||||
def width_mm(self) -> float:
|
||||
return float(self.position) / 1000.0 * float(self.stroke_mm)
|
||||
|
||||
def as_tuple(self) -> tuple[float, float, float, float, float]:
|
||||
return (
|
||||
1.0 if self.initialized else 0.0,
|
||||
float(self.grip_state),
|
||||
float(self.position),
|
||||
float(self.target_position),
|
||||
float(self.target_force),
|
||||
)
|
||||
|
||||
def summary(self) -> str:
|
||||
return (
|
||||
f"init={INIT_STATE_NAME.get(self.init_state, self.init_state)} "
|
||||
f"grip={GRIP_STATE_NAME.get(self.grip_state, self.grip_state)} "
|
||||
f"pos={self.position}/1000 ({self.width_mm:.2f} mm) "
|
||||
f"target={self.target_position} force={self.target_force}%"
|
||||
)
|
||||
|
||||
|
||||
class DhPgeaError(RuntimeError):
|
||||
pass
|
||||
|
||||
|
||||
WIRING_HINT = (
|
||||
"PC 已发出 Modbus，夹爪 0 字节返回。不是力/速度参数问题。按顺序查：\n"
|
||||
" 1) 夹爪 24 V DC 已上电（指示灯亮）\n"
|
||||
" 2) USB 转接头必须是 RS485（A/B），不能是 TTL 的 TX/RX\n"
|
||||
" 3) A↔A、B↔B；不通就对调 A/B\n"
|
||||
" 4) 先 Ctrl+C 停节点，再：bash ~/ros2/scripts/run_gripper.sh --cli scan"
|
||||
)
|
||||
|
||||
|
||||
def open_failure_text(port: str, exc: BaseException) -> str:
|
||||
"""串口打不开时把「被占用」和「没应答」分开。
|
||||
|
||||
独占锁失败（errno 11）以前和真没上电写成同一句「需要 24 V DC」，
|
||||
界面上看起来像缺自动上电，实际是别的进程还握着这个口。
|
||||
"""
|
||||
text = str(exc).lower()
|
||||
errno = getattr(exc, "errno", None)
|
||||
busy = errno in (11, 16) or any(
|
||||
token in text
|
||||
for token in (
|
||||
"temporarily unavailable",
|
||||
"device or resource busy",
|
||||
"could not exclusively lock",
|
||||
"resource busy",
|
||||
)
|
||||
)
|
||||
if busy:
|
||||
return (
|
||||
f"无法独占打开 {port}：{exc}。"
|
||||
"串口已被别的进程占用（夹爪驱动或状态读取）。"
|
||||
)
|
||||
return (
|
||||
f"无法打开 {port}：{exc}。"
|
||||
"需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
|
||||
)
|
||||
|
||||
|
||||
class DhPgeaGripper:
|
||||
"""面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
port: str = "/dev/ttyUSB0",
|
||||
baudrate: int = 115200,
|
||||
slave_id: int = 1,
|
||||
timeout_s: float = 0.25,
|
||||
stroke_mm: float = 20.0,
|
||||
rs485_rts: str = "",
|
||||
) -> None:
|
||||
self.port = port
|
||||
self.baudrate = int(baudrate)
|
||||
self.slave_id = int(slave_id)
|
||||
self.timeout_s = float(timeout_s)
|
||||
self.stroke_mm = float(stroke_mm)
|
||||
mode = str(rs485_rts or "").strip().lower()
|
||||
if mode in ("", "off", "none", "false", "0"):
|
||||
mode = ""
|
||||
if mode not in ("", "tx", "tx_inv"):
|
||||
raise DhPgeaError(f"rs485_rts 只能是 ''、tx 或 tx_inv，实际为 {rs485_rts!r}")
|
||||
self.rs485_rts = mode
|
||||
self._ser: Optional[serial.Serial] = None
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def open(self) -> None:
|
||||
if self._ser is not None and self._ser.is_open:
|
||||
return
|
||||
try:
|
||||
self._ser = serial.Serial(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
bytesize=serial.EIGHTBITS,
|
||||
parity=serial.PARITY_NONE,
|
||||
stopbits=serial.STOPBITS_ONE,
|
||||
timeout=self.timeout_s,
|
||||
write_timeout=self.timeout_s,
|
||||
xonxoff=False,
|
||||
rtscts=False,
|
||||
dsrdtr=False,
|
||||
exclusive=True,
|
||||
)
|
||||
except Exception as exc:
|
||||
raise DhPgeaError(open_failure_text(self.port, exc)) from exc
|
||||
try:
|
||||
self._ser.dtr = True
|
||||
except Exception:
|
||||
pass
|
||||
self._set_tx(False)
|
||||
self._ser.reset_input_buffer()
|
||||
self._ser.reset_output_buffer()
|
||||
time.sleep(0.05)
|
||||
|
||||
def close(self) -> None:
|
||||
if self._ser is not None:
|
||||
try:
|
||||
self._ser.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._ser = None
|
||||
|
||||
def __enter__(self) -> "DhPgeaGripper":
|
||||
self.open()
|
||||
return self
|
||||
|
||||
def __exit__(self, *args) -> None:
|
||||
self.close()
|
||||
|
||||
def write_register(self, index: int, value: int, retries: int = 3) -> bool:
|
||||
"""FC06 写，等待回显（对应官方 WriteRegisterFunc）。"""
|
||||
frame = _frame(self.slave_id, 0x06, index, int(value) & 0xFFFF)
|
||||
with self._lock:
|
||||
for _ in range(max(1, retries)):
|
||||
if self._transact(frame, 8) == frame:
|
||||
return True
|
||||
time.sleep(0.02)
|
||||
return False
|
||||
|
||||
def read_register(self, index: int, retries: int = 8) -> int:
|
||||
"""FC03 读一个保持寄存器（对应官方 ReadRegisterFunc）。"""
|
||||
req = _frame(self.slave_id, 0x03, index, 1)
|
||||
last: Optional[bytes] = None
|
||||
with self._lock:
|
||||
for _ in range(max(1, retries)):
|
||||
rsp = self._transact(req, 7)
|
||||
last = rsp
|
||||
if rsp is None or len(rsp) != 7:
|
||||
time.sleep(0.02)
|
||||
continue
|
||||
if crc16_modbus(rsp[:-2]) != (rsp[5] | (rsp[6] << 8)):
|
||||
continue
|
||||
if rsp[0] != (self.slave_id & 0xFF) or rsp[1] != 0x03:
|
||||
continue
|
||||
return (rsp[3] << 8) | (rsp[4] & 0xFF)
|
||||
hint = f" last={last.hex() if last else '(empty)'}"
|
||||
raise DhPgeaError(
|
||||
f"在 {self.port} 上 Modbus 读 0x{index:04X} 失败 "
|
||||
f"（无有效应答{hint}）。baud={self.baudrate} slave={self.slave_id} "
|
||||
f"rts={self.rs485_rts or 'off'}"
|
||||
)
|
||||
|
||||
def probe(self) -> int:
|
||||
"""短读一次初始化状态。夹爪无应答时会抛异常。"""
|
||||
return self.read_register(REG_INIT_STATE, retries=2)
|
||||
|
||||
def _set_tx(self, tx: bool) -> None:
|
||||
if not self.rs485_rts or self._ser is None:
|
||||
return
|
||||
try:
|
||||
if self.rs485_rts == "tx":
|
||||
self._ser.rts = bool(tx)
|
||||
else:
|
||||
self._ser.rts = not bool(tx)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _transact(self, frame: bytes, expect: int) -> Optional[bytes]:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
raise DhPgeaError("串口未打开")
|
||||
bit_s = 10.0 / max(1, self.baudrate)
|
||||
self._set_tx(True)
|
||||
if self.rs485_rts:
|
||||
time.sleep(2.0 * bit_s)
|
||||
self._ser.reset_input_buffer()
|
||||
self._ser.write(frame)
|
||||
self._ser.flush()
|
||||
if self.rs485_rts:
|
||||
time.sleep(len(frame) * bit_s + 0.0008)
|
||||
self._set_tx(False)
|
||||
deadline = time.monotonic() + self.timeout_s
|
||||
buf = bytearray()
|
||||
while time.monotonic() < deadline and len(buf) < expect:
|
||||
chunk = self._ser.read(expect - len(buf))
|
||||
if chunk:
|
||||
buf.extend(chunk)
|
||||
return bytes(buf) if buf else None
|
||||
|
||||
def initialization(self) -> bool:
|
||||
return self.write_register(REG_INIT, INIT_CMD)
|
||||
|
||||
def initialize(
|
||||
self, timeout_s: float = 20.0, poll_s: float = 0.2, *, force: bool = False
|
||||
) -> bool:
|
||||
"""写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。
|
||||
|
||||
``force=False`` 时，已经完成就不再写（节点启动不要每次都让手指找零）。
|
||||
命令行 ``init`` 必须 ``force=True``：状态字已经是完成时，不写 0xA5
|
||||
手指根本不会回零，看起来像初始化失效。
|
||||
"""
|
||||
state = self.get_init_state()
|
||||
if state == S_INIT_FINISHED and not force:
|
||||
return True
|
||||
if not self.initialization():
|
||||
raise DhPgeaError("初始化写 0x0100=0xA5 失败（无 Modbus 回显）")
|
||||
deadline = time.monotonic() + float(timeout_s)
|
||||
# force 时必须先看到状态离开「完成」，否则读到的还是写之前的旧值。
|
||||
saw_busy = not force
|
||||
while time.monotonic() < deadline:
|
||||
state = self.get_init_state()
|
||||
if state != S_INIT_FINISHED:
|
||||
saw_busy = True
|
||||
elif saw_busy:
|
||||
return True
|
||||
time.sleep(poll_s)
|
||||
raise DhPgeaError(
|
||||
f"初始化在 {timeout_s:.0f}s 后超时（state={state}）。"
|
||||
"首次上电时手指本应先回零一次。"
|
||||
)
|
||||
|
||||
def set_target_force(self, force_pct: int) -> bool:
|
||||
force_pct = int(max(20, min(100, force_pct)))
|
||||
return self.write_register(REG_FORCE, force_pct)
|
||||
|
||||
def set_target_speed(self, speed_pct: int) -> bool:
|
||||
speed_pct = int(max(1, min(100, speed_pct)))
|
||||
return self.write_register(REG_SPEED, speed_pct)
|
||||
|
||||
def _wait_command_echo(self, value: int, timeout_s: float = 0.6) -> bool:
|
||||
"""等到 0x0103 读回就是刚写进去的值。读不回就不要再写下一笔。"""
|
||||
deadline = time.monotonic() + float(timeout_s)
|
||||
while time.monotonic() < deadline:
|
||||
try:
|
||||
if self.get_target_position(retries=1) == int(value):
|
||||
return True
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.05)
|
||||
return False
|
||||
|
||||
def set_target_position(self, pos: int) -> bool:
|
||||
pos = int(max(0, min(1000, pos)))
|
||||
try:
|
||||
cmd = self.get_target_position(retries=2)
|
||||
except Exception:
|
||||
cmd = pos
|
||||
try:
|
||||
cur = self.get_current_position(retries=2)
|
||||
except Exception:
|
||||
cur = pos
|
||||
bridge = latched_position_break(cmd, cur, pos)
|
||||
if bridge is not None:
|
||||
if not self.write_register(REG_POSITION, bridge):
|
||||
return False
|
||||
if not self._wait_command_echo(bridge):
|
||||
return False
|
||||
# 读回只说明寄存器改了。再留一个控制周期，避免下一笔把中间值盖掉。
|
||||
time.sleep(0.15)
|
||||
return self.write_register(REG_POSITION, pos)
|
||||
|
||||
def wait_settle(self, target: int, timeout_s: float = 6.0) -> GripperState:
|
||||
"""等到到位或夹住，或超时返回最后一帧。
|
||||
|
||||
张开前状态字已经是 caught 时不能立刻返回：那是上一句命令的残留，
|
||||
手指还停在夹持位。
|
||||
"""
|
||||
t0 = time.monotonic()
|
||||
last = None
|
||||
seen_motion = False
|
||||
st = self.get_run_states()
|
||||
while time.monotonic() - t0 < timeout_s:
|
||||
st = self.get_run_states()
|
||||
if last is not None and st.position != last:
|
||||
seen_motion = True
|
||||
last = st.position
|
||||
if st.grip_state == S_GRIP_MOVING:
|
||||
seen_motion = True
|
||||
time.sleep(0.08)
|
||||
continue
|
||||
if grip_motion_settled(st.grip_state, st.position, target, seen_motion):
|
||||
return st
|
||||
time.sleep(0.08)
|
||||
return st
|
||||
|
||||
def get_init_state(self, retries: int = 8) -> int:
|
||||
return self.read_register(REG_INIT_STATE, retries=retries)
|
||||
|
||||
def get_grip_state(self, retries: int = 8) -> int:
|
||||
return self.read_register(REG_GRIP_STATE, retries=retries)
|
||||
|
||||
def get_current_position(self, retries: int = 8) -> int:
|
||||
return self.read_register(REG_CUR_POSITION, retries=retries)
|
||||
|
||||
def get_target_position(self, retries: int = 8) -> int:
|
||||
return self.read_register(REG_POSITION, retries=retries)
|
||||
|
||||
def get_target_force(self, retries: int = 8) -> int:
|
||||
return self.read_register(REG_FORCE, retries=retries)
|
||||
|
||||
def get_run_states(self, retries: int = 8) -> GripperState:
|
||||
return GripperState(
|
||||
init_state=self.get_init_state(retries=retries),
|
||||
grip_state=self.get_grip_state(retries=retries),
|
||||
position=self.get_current_position(retries=retries),
|
||||
target_position=self.get_target_position(retries=retries),
|
||||
target_force=self.get_target_force(retries=retries),
|
||||
stroke_mm=self.stroke_mm,
|
||||
)
|
||||
|
||||
def open_jaw(self, force: int = 50, speed: int = 50) -> bool:
|
||||
ok = self.set_target_force(force)
|
||||
ok = self.set_target_speed(speed) and ok
|
||||
return self.set_target_position(1000) and ok
|
||||
|
||||
def close_jaw(self, force: int = 50, speed: int = 50) -> bool:
|
||||
ok = self.set_target_force(force)
|
||||
ok = self.set_target_speed(speed) and ok
|
||||
return self.set_target_position(0) and ok
|
||||
|
||||
def move(self, position: int, force: int = 50, speed: int = 50) -> bool:
|
||||
ok = self.set_target_force(force)
|
||||
ok = self.set_target_speed(speed) and ok
|
||||
return self.set_target_position(position) and ok
|
||||
|
||||
|
||||
def _selfcheck_crc() -> None:
|
||||
"""官方简短手册示例：设置位置 500。"""
|
||||
frame = _frame(1, 0x06, 0x0103, 500)
|
||||
expect = bytes([0x01, 0x06, 0x01, 0x03, 0x01, 0xF4, 0x78, 0x21])
|
||||
if frame != expect:
|
||||
raise AssertionError(f"CRC 不匹配：{frame.hex()} != {expect.hex()}")
|
||||
|
||||
|
||||
def _site_defaults() -> dict:
|
||||
try:
|
||||
from pathlib import Path
|
||||
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import load_site # type: ignore
|
||||
|
||||
g = (load_site().get("gripper") or {})
|
||||
return {
|
||||
"port": str(g.get("port") or "/dev/ttyUSB0"),
|
||||
"baudrate": int(g.get("baud") or 115200),
|
||||
"slave_id": int(g.get("slave_id") or 1),
|
||||
"stroke_mm": float(g.get("stroke_mm") or 20.0),
|
||||
"force": int(g.get("force_pct") or 50),
|
||||
"speed": int(g.get("speed_pct") or 50),
|
||||
"rs485_rts": str(g.get("rs485_rts") or ""),
|
||||
}
|
||||
except Exception:
|
||||
return {
|
||||
"port": "/dev/ttyUSB0",
|
||||
"baudrate": 115200,
|
||||
"slave_id": 1,
|
||||
"stroke_mm": 20.0,
|
||||
"force": 50,
|
||||
"speed": 50,
|
||||
"rs485_rts": "",
|
||||
}
|
||||
|
||||
|
||||
def _scan_bus(port: str) -> int:
|
||||
"""遍历波特率 × 从站号 × RTS，直到某个组合有应答。"""
|
||||
bauds = (115200, 57600, 38400, 19200, 9600)
|
||||
slaves = (1, 2, 3)
|
||||
rts_modes = ("", "tx", "tx_inv")
|
||||
print(f"扫描 {port}（请先停掉 ROS 节点 —— 该串口是独占的）")
|
||||
for baud in bauds:
|
||||
for slave in slaves:
|
||||
for rts in rts_modes:
|
||||
label = f"baud={baud} slave={slave} rts={rts or 'off'}"
|
||||
g = DhPgeaGripper(
|
||||
port=port,
|
||||
baudrate=baud,
|
||||
slave_id=slave,
|
||||
timeout_s=0.08,
|
||||
rs485_rts=rts,
|
||||
)
|
||||
try:
|
||||
g.open()
|
||||
state = g.probe()
|
||||
print(f"OK {label} init_state={state}")
|
||||
print(f" → site.yaml: baud: {baud} slave_id: {slave} rs485_rts: \"{rts}\"")
|
||||
g.close()
|
||||
return 0
|
||||
except DhPgeaError:
|
||||
print(f" miss {label}")
|
||||
finally:
|
||||
g.close()
|
||||
print("所有组合都无应答。", file=sys.stderr)
|
||||
print(WIRING_HINT, file=sys.stderr)
|
||||
return 1
|
||||
|
||||
|
||||
def ctrl_values(
|
||||
cmd: str,
|
||||
value: str = "",
|
||||
force: int = 50,
|
||||
speed: int = 50,
|
||||
) -> Optional[tuple[float, float, float, float]]:
|
||||
"""节点已经占着串口时，改发 ``/gripper/ctrl`` 的四个数。
|
||||
|
||||
顺序是初始化、位置千分比、力度、速度。``probe`` / ``status`` 只读，返回 None。
|
||||
"""
|
||||
name = str(cmd or "").strip().lower()
|
||||
if name in ("probe", "status"):
|
||||
return None
|
||||
if name == "init":
|
||||
return (1.0, 0.0, float(force), float(speed))
|
||||
if name == "open":
|
||||
pos = 1000
|
||||
elif name == "close":
|
||||
pos = 0
|
||||
elif name in ("pos", "move"):
|
||||
if str(value).strip() == "":
|
||||
raise ValueError("pos 需要 0–1000")
|
||||
pos = int(round(float(value)))
|
||||
if pos < 0 or pos > 1000:
|
||||
raise ValueError("位置只接受 0–1000")
|
||||
else:
|
||||
raise ValueError(f"节点在线时不支持 {name}")
|
||||
return (0.0, float(pos), float(force), float(speed))
|
||||
|
||||
|
||||
def _cli(argv: Optional[list[str]] = None) -> int:
|
||||
site = _site_defaults()
|
||||
ap = argparse.ArgumentParser(
|
||||
description="DH PGEA Modbus 命令行工具（官方寄存器映射，不用 Flexiv RDK）"
|
||||
)
|
||||
ap.add_argument("--port", default=site["port"])
|
||||
ap.add_argument("--baud", type=int, default=site["baudrate"])
|
||||
ap.add_argument("--slave", type=int, default=site["slave_id"])
|
||||
ap.add_argument("--force", type=int, default=site["force"])
|
||||
ap.add_argument("--speed", type=int, default=site["speed"])
|
||||
ap.add_argument("--rts", default=site.get("rs485_rts") or "", help="RS485 DE：空 | tx | tx_inv")
|
||||
ap.add_argument(
|
||||
"cmd",
|
||||
choices=["probe", "init", "status", "open", "close", "pos", "selfcheck", "scan"],
|
||||
)
|
||||
ap.add_argument("value", nargs="?", default="", help="'pos' 命令的位置，0–1000")
|
||||
args = ap.parse_args(argv)
|
||||
|
||||
if args.cmd == "selfcheck":
|
||||
_selfcheck_crc()
|
||||
print("CRC 自检通过（01 06 01 03 01 F4 78 21）")
|
||||
return 0
|
||||
if args.cmd == "scan":
|
||||
return _scan_bus(args.port)
|
||||
|
||||
grip = DhPgeaGripper(
|
||||
port=args.port,
|
||||
baudrate=args.baud,
|
||||
slave_id=args.slave,
|
||||
stroke_mm=float(site["stroke_mm"]),
|
||||
rs485_rts=args.rts,
|
||||
)
|
||||
try:
|
||||
grip.open()
|
||||
except DhPgeaError as exc:
|
||||
print(f"错误：{exc}", file=sys.stderr)
|
||||
return 1
|
||||
|
||||
try:
|
||||
if args.cmd == "probe":
|
||||
st = grip.get_run_states(retries=2)
|
||||
print(st.summary())
|
||||
return 0
|
||||
if args.cmd == "init":
|
||||
print("正在初始化（手指回零，已初始化也会再做一次）…")
|
||||
grip.initialize(force=True)
|
||||
print(grip.get_run_states().summary())
|
||||
return 0
|
||||
if args.cmd == "status":
|
||||
print(grip.get_run_states().summary())
|
||||
return 0
|
||||
if args.cmd == "open":
|
||||
if not grip.open_jaw(args.force, args.speed):
|
||||
raise DhPgeaError("张开写入失败")
|
||||
st = grip.wait_settle(1000)
|
||||
print(st.summary())
|
||||
if abs(st.position - 1000) > 25:
|
||||
print(
|
||||
"张开失败：手指没有离开夹持位。"
|
||||
"若仍是 grip=caught，先取下工件再执行 init。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 1
|
||||
return 0
|
||||
if args.cmd == "close":
|
||||
if not grip.close_jaw(args.force, args.speed):
|
||||
raise DhPgeaError("闭合写入失败")
|
||||
st = grip.wait_settle(0)
|
||||
print(st.summary())
|
||||
# 夹住物体时位置停在厚度上，grip=caught 算成功。停在张开位则失败。
|
||||
if st.grip_state != S_GRIP_CAUGHT and abs(st.position) > 25:
|
||||
print("闭合失败：手指没有朝合拢方向离开原位。", file=sys.stderr)
|
||||
return 1
|
||||
return 0
|
||||
if args.cmd == "pos":
|
||||
if args.value == "":
|
||||
print("用法：pos <0-1000>", file=sys.stderr)
|
||||
return 2
|
||||
pos = int(float(args.value))
|
||||
if not grip.move(pos, args.force, args.speed):
|
||||
raise DhPgeaError("位置写入失败")
|
||||
st = grip.wait_settle(pos)
|
||||
print(st.summary())
|
||||
grasped_short = st.grip_state == S_GRIP_CAUGHT and st.position > pos
|
||||
if abs(st.position - pos) > 25 and not grasped_short:
|
||||
print(
|
||||
f"位置 {pos} 没有到位（仍在 {st.position}）。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 1
|
||||
return 0
|
||||
except DhPgeaError as exc:
|
||||
print(f"错误：{exc}", file=sys.stderr)
|
||||
print(WIRING_HINT, file=sys.stderr)
|
||||
return 1
|
||||
finally:
|
||||
grip.close()
|
||||
return 0
|
||||
|
||||
|
||||
def main(argv: Optional[list[str]] = None) -> int:
|
||||
return _cli(argv)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,307 @@
|
||||
#!/usr/bin/env python3
|
||||
"""DH PGEA 的 ROS 2 节点（官方 Modbus RTU，不用 Flexiv 夹爪）。
|
||||
|
||||
话题（布局与 DH-Robotics/dh_gripper_ros 一致）：
|
||||
/gripper/ctrl std_msgs/Float32MultiArray
|
||||
data: [initialize, position, force, speed]
|
||||
initialize>0.5 → 写 0xA5；否则设置力/速度/位置
|
||||
/gripper/states std_msgs/Float32MultiArray
|
||||
data: [initialized, grip_state, position, target_pos, target_force]
|
||||
/gripper/joint_states sensor_msgs/JointState
|
||||
|
||||
服务（std_srvs/Trigger）：
|
||||
/gripper/initialize /gripper/open /gripper/close
|
||||
|
||||
不要设置 Flexiv bringup 的 load_gripper:=true —— 那只是 Flexiv-GN01 用的。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
import math
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Float32, Float32MultiArray, MultiArrayDimension, MultiArrayLayout
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from arm_application.dh_pgea import (
|
||||
DhPgeaError,
|
||||
DhPgeaGripper,
|
||||
GripperState,
|
||||
WIRING_HINT,
|
||||
)
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
|
||||
|
||||
def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]:
|
||||
data = list(msg.data)
|
||||
while len(data) < 4:
|
||||
data.append(0.0)
|
||||
return float(data[0]), float(data[1]), float(data[2]), float(data[3])
|
||||
|
||||
|
||||
class DhPgeaNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("dh_pgea", namespace="/gripper")
|
||||
self.declare_parameter("port", "/dev/ttyUSB0")
|
||||
self.declare_parameter("baud", 115200)
|
||||
self.declare_parameter("slave_id", 1)
|
||||
self.declare_parameter("stroke_mm", 20.0)
|
||||
self.declare_parameter("force_pct", 50)
|
||||
self.declare_parameter("speed_pct", 50)
|
||||
self.declare_parameter("auto_init", False)
|
||||
self.declare_parameter("state_hz", 20.0)
|
||||
self.declare_parameter("open_position", 1000)
|
||||
self.declare_parameter("close_position", 0)
|
||||
self._model = param_as_str(self, "model", "PGEA-100-20-W")
|
||||
self._sn = param_as_str(self, "serial_number", "")
|
||||
self._rts = param_as_str(self, "rs485_rts", "")
|
||||
|
||||
self._port = str(self.get_parameter("port").value)
|
||||
self._baud = int(self.get_parameter("baud").value)
|
||||
self._slave = int(self.get_parameter("slave_id").value)
|
||||
self._stroke = float(self.get_parameter("stroke_mm").value)
|
||||
if not math.isfinite(self._stroke) or self._stroke <= 0:
|
||||
raise DhPgeaError("夹爪行程必须是正数")
|
||||
self._force = int(self.get_parameter("force_pct").value)
|
||||
self._speed = int(self.get_parameter("speed_pct").value)
|
||||
_auto = self.get_parameter("auto_init").value
|
||||
if isinstance(_auto, str):
|
||||
self._auto_init = _auto.strip().lower() in ("1", "true", "yes")
|
||||
else:
|
||||
self._auto_init = bool(_auto)
|
||||
self._open_pos = int(self.get_parameter("open_position").value)
|
||||
self._close_pos = int(self.get_parameter("close_position").value)
|
||||
hz = float(self.get_parameter("state_hz").value)
|
||||
|
||||
self._grip = DhPgeaGripper(
|
||||
port=self._port,
|
||||
baudrate=self._baud,
|
||||
slave_id=self._slave,
|
||||
stroke_mm=self._stroke,
|
||||
rs485_rts=self._rts,
|
||||
)
|
||||
self._state = GripperState(stroke_mm=self._stroke)
|
||||
self._cmd_lock = threading.Lock()
|
||||
|
||||
self._pub_state = self.create_publisher(Float32MultiArray, "states", 10)
|
||||
self._pub_typed = self.create_publisher(GripperStatus, "status", 10)
|
||||
self.create_service(GripperExecute, "execute", self._srv_execute)
|
||||
self._pub_js = self.create_publisher(JointState, "joint_states", 10)
|
||||
self.create_subscription(Float32MultiArray, "ctrl", self._on_ctrl, 10)
|
||||
self.create_subscription(Float32, "set_position", self._on_set_pos, 10)
|
||||
|
||||
self.create_service(Trigger, "initialize", self._srv_init)
|
||||
self.create_service(Trigger, "open", self._srv_open)
|
||||
self.create_service(Trigger, "close", self._srv_close)
|
||||
|
||||
self.get_logger().info(
|
||||
f"DH {self._model} SN={self._sn or '?'} "
|
||||
f"{self._port} @{self._baud} slave={self._slave} rts={self._rts or 'off'} "
|
||||
f"（官方 Modbus RTU；未使用 Flexiv RDK 夹爪 API）"
|
||||
)
|
||||
try:
|
||||
self._grip.open()
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
raise
|
||||
|
||||
self._online = False
|
||||
try:
|
||||
self._grip.probe()
|
||||
self._online = True
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
for line in WIRING_HINT.splitlines():
|
||||
self.get_logger().error(line)
|
||||
self.get_logger().error("会持续重试；在总线恢复应答前 open/close 都会失败")
|
||||
|
||||
if self._auto_init and self._online:
|
||||
try:
|
||||
self._grip.initialize()
|
||||
self.get_logger().info("夹爪已初始化")
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(f"auto_init 失败：{exc}")
|
||||
self._online = False
|
||||
|
||||
period = 1.0 / max(1.0, hz)
|
||||
self._timer = self.create_timer(period, self._on_timer)
|
||||
|
||||
def destroy_node(self) -> bool:
|
||||
try:
|
||||
self._grip.close()
|
||||
except Exception:
|
||||
pass
|
||||
return super().destroy_node()
|
||||
|
||||
def _apply_move(self, position: int, force: Optional[int] = None, speed: Optional[int] = None) -> str:
|
||||
with self._cmd_lock:
|
||||
f = self._force if force is None else int(force)
|
||||
s = self._speed if speed is None else int(speed)
|
||||
if force is not None:
|
||||
self._force = f
|
||||
if speed is not None:
|
||||
self._speed = s
|
||||
if not self._grip.move(int(position), f, s):
|
||||
raise DhPgeaError("移动写入失败")
|
||||
return f"pos={int(position)} force={f}% speed={s}%"
|
||||
|
||||
def _on_ctrl(self, msg: Float32MultiArray) -> None:
|
||||
initialize, position, force, speed = _as_ctrl_array(msg)
|
||||
try:
|
||||
if initialize > 0.5:
|
||||
self._grip.initialize()
|
||||
self.get_logger().info("通过 /gripper/ctrl 触发初始化")
|
||||
return
|
||||
f = int(force) if force > 0 else self._force
|
||||
s = int(speed) if speed > 0 else self._speed
|
||||
info = self._apply_move(int(position), f, s)
|
||||
self.get_logger().info(f"ctrl {info}")
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
|
||||
def _on_set_pos(self, msg: Float32) -> None:
|
||||
try:
|
||||
info = self._apply_move(int(msg.data))
|
||||
self.get_logger().info(f"set_position {info}")
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
|
||||
def _srv_init(self, _req, resp):
|
||||
try:
|
||||
self._grip.initialize()
|
||||
resp.success = True
|
||||
resp.message = self._grip.get_run_states().summary()
|
||||
except DhPgeaError as exc:
|
||||
resp.success = False
|
||||
resp.message = str(exc)
|
||||
return resp
|
||||
|
||||
def _srv_open(self, _req, resp):
|
||||
try:
|
||||
info = self._apply_move(self._open_pos)
|
||||
resp.success = True
|
||||
resp.message = info
|
||||
except DhPgeaError as exc:
|
||||
resp.success = False
|
||||
resp.message = str(exc)
|
||||
return resp
|
||||
|
||||
def _srv_close(self, _req, resp):
|
||||
try:
|
||||
info = self._apply_move(self._close_pos)
|
||||
resp.success = True
|
||||
resp.message = info
|
||||
except DhPgeaError as exc:
|
||||
resp.success = False
|
||||
resp.message = str(exc)
|
||||
return resp
|
||||
|
||||
def _typed_status(self, error=""):
|
||||
state = self._state
|
||||
msg = GripperStatus()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.name = self._model
|
||||
msg.gripper_type = "parallel"
|
||||
msg.connected = self._online and not error
|
||||
msg.initialized = bool(state.initialized)
|
||||
msg.is_moving = state.grip_state == 0
|
||||
msg.is_gripped = state.grip_state == 2
|
||||
msg.state = "ERROR" if error else "MOVING" if msg.is_moving else "IDLE"
|
||||
msg.position = float(state.width_mm)
|
||||
msg.finger_position_per_mille = float(state.position)
|
||||
msg.gripper_state = int(state.grip_state)
|
||||
msg.rotation_state = -1
|
||||
msg.velocity = float(self._speed)
|
||||
msg.force = float(state.target_force)
|
||||
msg.error_msg = error
|
||||
return msg
|
||||
|
||||
def _srv_execute(self, request, response):
|
||||
command = request.command
|
||||
try:
|
||||
values = (command.position, command.force, command.speed, command.timeout)
|
||||
if not all(math.isfinite(v) for v in values):
|
||||
raise ValueError("命令参数必须是有限数值")
|
||||
if command.command in ("connect", "probe"):
|
||||
self._state = self._grip.get_run_states(retries=1)
|
||||
self._online = True
|
||||
elif command.command == "init":
|
||||
# Initialization can move fingers; never infer it from connect/probe.
|
||||
self._grip.initialize()
|
||||
self._state = self._grip.get_run_states(retries=1)
|
||||
elif command.command in ("move", "open", "close"):
|
||||
if not 20 <= command.force <= 100 or not 1 <= command.speed <= 100:
|
||||
raise ValueError("力只接受 20–100%，速度只接受 1–100%")
|
||||
if not 0 <= command.position <= self._stroke:
|
||||
raise ValueError("位置超出夹爪行程")
|
||||
position = self._open_pos if command.command == "open" else self._close_pos if command.command == "close" else round(command.position/self._stroke*1000)
|
||||
self._apply_move(position, round(command.force), round(command.speed))
|
||||
if command.wait_complete:
|
||||
self._state = self._grip.wait_settle(position, timeout_s=min(30., max(.1,command.timeout)))
|
||||
else:
|
||||
raise ValueError("PGEA 不支持此命令或旋转轴")
|
||||
response.success = True
|
||||
response.message = "状态已读取" if command.command in ("connect", "probe") else "命令已写入驱动"
|
||||
except (DhPgeaError, ValueError) as exc:
|
||||
response.success = False
|
||||
response.message = str(exc)
|
||||
response.status = self._typed_status("" if response.success else response.message)
|
||||
return response
|
||||
|
||||
def _on_timer(self) -> None:
|
||||
try:
|
||||
st = self._grip.get_run_states(retries=2 if self._online else 1)
|
||||
self._state = st
|
||||
if not self._online:
|
||||
self._online = True
|
||||
self.get_logger().info("夹爪总线已恢复")
|
||||
except DhPgeaError as exc:
|
||||
self._online = False
|
||||
self.get_logger().warn(str(exc), throttle_duration_sec=5.0)
|
||||
self._pub_typed.publish(self._typed_status(str(exc)))
|
||||
return
|
||||
|
||||
self._pub_typed.publish(self._typed_status())
|
||||
arr = Float32MultiArray()
|
||||
arr.layout = MultiArrayLayout(
|
||||
dim=[MultiArrayDimension(label="dh_pgea", size=5, stride=5)],
|
||||
data_offset=0,
|
||||
)
|
||||
arr.data = list(st.as_tuple())
|
||||
self._pub_state.publish(arr)
|
||||
|
||||
js = JointState()
|
||||
js.header.stamp = self.get_clock().now().to_msg()
|
||||
js.name = ["gripper_finger1_joint"]
|
||||
# 官方 ROS 驱动把 0–1000 映射到约 0–0.637 rad；我们这里也发布 mm。
|
||||
js.position = [float(st.width_mm) * 0.001]
|
||||
js.effort = [float(st.target_force)]
|
||||
self._pub_js.publish(js)
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = None
|
||||
try:
|
||||
node = DhPgeaNode()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
except DhPgeaError:
|
||||
raise SystemExit(1)
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,200 @@
|
||||
#!/usr/bin/env python3
|
||||
"""跟踪点的三道守卫：**位置合理性** + **每帧位移上限** + **远框重捕获确认**。
|
||||
|
||||
为什么需要它们（2026-10-08 真机 arm 89，一阶段跑 150s 没锁上）
|
||||
------------------------------------------------------------
|
||||
``/tmp/pipeline_stage1.log`` 原话。真 k0 一开始是对的::
|
||||
|
||||
764.79 跟踪 keypoint conf=0.87 uv=(410,246) z=0.299 ← 真 k0（画面中偏右）
|
||||
|
||||
然后在 1 秒内被一串**假 k0** 顶掉（全都是高置信度）::
|
||||
|
||||
766.96 重新捕获 conf=0.58 dpx=480
|
||||
766.96 跟踪 keypoint conf=0.58 uv=(76,75) z=0.392 ← 贴左上角
|
||||
767.75 跟踪 keypoint conf=0.23 uv=(169,107) z_ok=0 深度过期 box_z hold_near
|
||||
768.16 跟踪 keypoint conf=0.53 uv=(143,92) z=0.376
|
||||
768.78 跟踪 keypoint conf=0.73 uv=(12,30) k0(ok=1551 near=620 借深度) z=0.276
|
||||
769.66 重新捕获 conf=0.22 dpx=207
|
||||
770.07 跟踪 keypoint conf=0.73 uv=(431,226) k0(ok=19440 near=7776 借深度) z=0.271
|
||||
|
||||
伺服于是以 0.45m/s 追着它们跑：``ee_xy`` home ``(0.686,-0.113)`` → ``(0.354,-0.294)``
|
||||
→ ``(0.083,-0.515)``（工作盒角落），1.2s 后看门狗急停；工件已在半米外，
|
||||
剩下 148s 全是"检测漏检"。三条结论：
|
||||
|
||||
1. **置信度挡不住**：假 k0 的 conf 是 0.53~0.73，真 k0 是 0.87 —— 没有可分区间。
|
||||
但假 k0 有一个共同特征：**落在画面边上**（``uv=(12,30)`` / ``(76,75)`` /
|
||||
``(143,92)``），而孔心长在工件上、不可能贴在画面边缘；
|
||||
2. **借深度把"没有深度"伪装成"有深度"**：``k0(… 借深度)`` 的 z 是从工件框里
|
||||
借的，看着很合理（0.27~0.39m），于是"物理量程"这道守卫也失效了。而
|
||||
**借来的深度意味着该像素处读不到自己的深度** ⇒ 那个点的绝对 XY 不可靠，
|
||||
不该用来**重定位**（换一个跟踪位置）；
|
||||
3. **位移不受限**：真 k0 到假 k0 一帧能差 480px / 400mm，伺服一次就全速扑过去。
|
||||
|
||||
三道守卫各挡一层，全都是"能真的失败"的判据（见 test_feature_guard_*.py 的负向用例）：
|
||||
|
||||
``kpt_plausible``
|
||||
k0 必须落在自己的检测框内（含少量余量）、且不许贴画面边缘。违反 ⇒ 本帧的
|
||||
k0 **位置**不可信（不是"没有目标"），交回调用方走"按框上相对偏移重建"的
|
||||
连续性路径（``perception_node._keypoint_feature`` 本来就有这条路）。
|
||||
``TrackSlewGuard``
|
||||
已发布跟踪点的**每帧位移上限**。工件不动、相机 30Hz，真实观测一帧不可能走
|
||||
4cm（``feature_jump_guard`` 实测真实运动 ≤15mm/帧）。超过上限：观测可信就
|
||||
**夹到上限**（限速跟随，不丢目标），观测来自借深度就**整帧不采信**。
|
||||
``FarReacquireConfirm``
|
||||
"远框重新捕获"（``dpx`` 大于门限）必须连续 ``confirm_frames`` 帧指向同一处
|
||||
才采信 —— 一帧的远框更可能是模型在别的东西上误报，而不是工件真的被挪走了。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _xy_dist(a: np.ndarray, b: np.ndarray) -> float:
|
||||
"""两个 XY 点的距离（米/像素都行，单位跟随入参）。单一实现，别各写一份。"""
|
||||
return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
|
||||
|
||||
|
||||
def kpt_plausible(
|
||||
u: float,
|
||||
v: float,
|
||||
box: Optional[Sequence[float]],
|
||||
w: int,
|
||||
h: int,
|
||||
*,
|
||||
border_px: float = 6.0,
|
||||
box_margin_frac: float = 0.10,
|
||||
) -> Optional[str]:
|
||||
"""k0 的**像素位置**是否可信。返回 None = 可信；否则返回拒绝原因。
|
||||
|
||||
``box`` 是同一帧的工件检测框 ``(x0,y0,x1,y1)``；``w,h`` 是彩色图宽高。
|
||||
"""
|
||||
if not (np.isfinite(u) and np.isfinite(v)):
|
||||
return "k0 像素非有限值"
|
||||
if box is None:
|
||||
# 没有框就没法判断 k0 是否落在工件上 —— 宁可当不可信（走连续性路径）。
|
||||
return "没有检测框，无法判断 k0 是否落在工件上"
|
||||
b = np.asarray(box, dtype=float).reshape(-1)
|
||||
if b.size < 4:
|
||||
return "检测框字段不完整"
|
||||
x0, y0, x1, y1 = (float(vv) for vv in b[:4])
|
||||
bw, bh = abs(x1 - x0), abs(y1 - y0)
|
||||
if not (np.isfinite(bw) and np.isfinite(bh)) or bw <= 1e-6 or bh <= 1e-6:
|
||||
return "检测框尺寸无效"
|
||||
mx, my = box_margin_frac * bw, box_margin_frac * bh
|
||||
if u < x0 - mx or u > x1 + mx or v < y0 - my or v > y1 + my:
|
||||
return (
|
||||
f"k0 ({u:.0f},{v:.0f}) 落在检测框 "
|
||||
f"[{x0:.0f},{y0:.0f},{x1:.0f},{y1:.0f}] 之外"
|
||||
)
|
||||
if w > 0 and h > 0:
|
||||
if u <= border_px or v <= border_px or u >= w - 1 - border_px or v >= h - 1 - border_px:
|
||||
return (
|
||||
f"k0 ({u:.0f},{v:.0f}) 贴画面边缘（{w}x{h}，余量 {border_px:.0f}px）"
|
||||
"—— 孔心长在工件上，不可能在画面边上"
|
||||
)
|
||||
return None
|
||||
|
||||
|
||||
class TrackSlewGuard:
|
||||
"""已发布跟踪点的每帧位移上限。
|
||||
|
||||
单实例服务**一个**跟踪点；调用方负责在"没有目标/重置"时 :meth:`reset`。
|
||||
"""
|
||||
|
||||
def __init__(self, max_step_m: float) -> None:
|
||||
# 只有**一个**门限，故意的：观测可信就限速跟随，借深度就整帧不采信。
|
||||
# 不做成两个门限是因为"借深度"只影响"能不能重定位"，不影响"小步跟不跟" ——
|
||||
# 两个门限会立刻变成"哪个该调、调到多少"的扯皮（且没有实测支撑）。
|
||||
self.max_step_m = float(max_step_m)
|
||||
self._last: Optional[np.ndarray] = None
|
||||
self.clamped_total = 0
|
||||
self.rejected_borrowed_total = 0
|
||||
|
||||
def reset(self) -> None:
|
||||
self._last = None
|
||||
|
||||
def update(
|
||||
self, p_xy: Optional[Sequence[float]], *, borrowed: bool = False
|
||||
) -> Tuple[Optional[np.ndarray], Optional[str]]:
|
||||
"""喂一次观测，返回 ``(采信的点, 拒绝原因)``。
|
||||
|
||||
- 第一次观测（没有历史）：原样采信（冷启动没有更好的信息，见服务节点的
|
||||
照常起步）。
|
||||
- 位移 ≤ 上限：原样采信。
|
||||
- 位移 > 上限且 ``borrowed``：整帧不采信（返回 ``None`` + 原因）——
|
||||
借来的深度意味着这个像素读不到自己的深度，绝对位置不足以用来重定位。
|
||||
- 位移 > 上限且观测可信：**夹到上限**（限速跟随），不丢目标。
|
||||
"""
|
||||
if p_xy is None:
|
||||
return None, "本帧没有观测"
|
||||
p = np.asarray(p_xy, dtype=float).reshape(2)
|
||||
if not np.all(np.isfinite(p)):
|
||||
return None, "跟踪点非有限值"
|
||||
if self._last is None:
|
||||
self._last = p.copy()
|
||||
return p, None
|
||||
d = _xy_dist(p, self._last)
|
||||
if d <= self.max_step_m:
|
||||
self._last = p.copy()
|
||||
return p, None
|
||||
if borrowed:
|
||||
self.rejected_borrowed_total += 1
|
||||
return None, (
|
||||
f"借深度的观测要求位移 {d*1000:.1f}mm > 上限 "
|
||||
f"{self.max_step_m*1000:.0f}mm —— 判误检，本帧不采信"
|
||||
)
|
||||
step = self._last + (p - self._last) * (self.max_step_m / d)
|
||||
self._last = step.copy()
|
||||
self.clamped_total += 1
|
||||
return step, (
|
||||
f"位移 {d*1000:.1f}mm > 上限 {self.max_step_m*1000:.0f}mm —— "
|
||||
"夹到上限限速跟随"
|
||||
)
|
||||
|
||||
|
||||
class FarReacquireConfirm:
|
||||
"""远框"重新捕获"的多帧确认。
|
||||
|
||||
``dpx`` 是候选框中心与上一帧框中心的像素距离。超过 ``far_px`` 的候选必须
|
||||
连续 ``confirm_frames`` 帧落在同一处（相互间距 ≤ ``far_px``）才放行。
|
||||
"""
|
||||
|
||||
def __init__(self, far_px: float, confirm_frames: int = 3) -> None:
|
||||
self.far_px = float(far_px)
|
||||
self.confirm_frames = max(1, int(confirm_frames))
|
||||
self._cand: Optional[np.ndarray] = None
|
||||
self._count = 0
|
||||
|
||||
def reset(self) -> None:
|
||||
self._cand = None
|
||||
self._count = 0
|
||||
|
||||
def allow(
|
||||
self, center_xy: Optional[Sequence[float]], dpx: float, *, near_px: float
|
||||
) -> Tuple[bool, Optional[str]]:
|
||||
"""返回 ``(是否放行, 拒绝原因)``。``near_px`` 是"不算远框"的门限。"""
|
||||
if center_xy is None:
|
||||
self.reset()
|
||||
return True, None
|
||||
c = np.asarray(center_xy, dtype=float).reshape(2)
|
||||
if not np.all(np.isfinite(c)):
|
||||
return False, "候选框中心非有限值"
|
||||
if float(dpx) <= float(near_px):
|
||||
# 近框：正常跟踪，不需要确认
|
||||
self.reset()
|
||||
return True, None
|
||||
if self._cand is None or _xy_dist(c, self._cand) > self.far_px:
|
||||
self._cand = c.copy()
|
||||
self._count = 1
|
||||
else:
|
||||
self._count += 1
|
||||
if self._count >= self.confirm_frames:
|
||||
self.reset()
|
||||
return True, None
|
||||
return False, (
|
||||
f"远框重捕获 dpx={float(dpx):.0f}px 需连续 {self.confirm_frames} 帧一致"
|
||||
f"（当前 {self._count} 帧）—— 本帧不采信"
|
||||
)
|
||||
@@ -0,0 +1,98 @@
|
||||
#!/usr/bin/env python3
|
||||
"""跟踪点"假跳变"的确认门限：单帧大跳不采信，连续 N 帧一致才认。
|
||||
|
||||
为什么需要它（2026-10-08 真机实测，一阶段跑 150s 没锁上）
|
||||
--------------------------------------------------------
|
||||
arm 89 的日志原话：
|
||||
|
||||
0659.551 死区保持 |e_xy|=3.0mm ← 其实已经锁定！误差只剩 3mm
|
||||
0659.598 k0 置信度 0.01 < 0.30，本帧不用它
|
||||
0659.604 HANDEYE snap feat_base=(0.675,-0.099) ← 跟踪点瞬间横移 89mm
|
||||
0660.121 v=(-0.206,+0.400) |e_xy|=130mm ← 伺服以 0.45m/s 去追这个假点
|
||||
0660.561 过期保持 —— 不跟踪冻结的检测框 ← 画面糊掉，目标彻底丢失
|
||||
|
||||
原来的写法是"跳变超过 ``predict_jump_m`` 就 ``reset()``" —— 等于**单帧就采信**。
|
||||
可是相机 30Hz、工件不动，真实特征点一帧不可能跳 89mm（实测的真实运动
|
||||
在 15mm/帧 以内）。所以"大跳"几乎必然是检测/兜底产生的假点，不该一路追过去。
|
||||
|
||||
判据
|
||||
----
|
||||
- 位移 ≤ ``jump_m``：正常观测（``near``），照常滤波更新。
|
||||
- 位移 > ``jump_m``：进入"待确认"。此后**连续 ``confirm_frames`` 帧**都要落在
|
||||
同一个新位置附近（相互间距 ≤ ``jump_m``），才判为真实重定位（``jump``）。
|
||||
- 只要中间有一帧不对上（乱跳），计数清零、重新开始 —— 乱跳永远等不到确认。
|
||||
|
||||
这也顺手挡住"感知层 k0 掉信后乱跳"那类现象：宁可原地不动，也不要追着假点跑。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
# ``servo_node._update_feat_track`` 的三种判定结果。
|
||||
# 为什么要显式区分"没有目标"：perception 会把误检点也发过来（2026-10-08 真机，
|
||||
# 模型在运动模糊下把特征点丢到基座后方 -0.10,-0.65，工件其实在 0.5,-0.12），
|
||||
# 这种点在物理上不可能 —— 必须当"没看到目标"处理，而不是"钉在原地"。
|
||||
ACCEPT = "accept" # 采信这次观测
|
||||
HOLD = "hold" # 疑似假跳 —— 位置钉住不动
|
||||
REJECT = "reject" # 物理上不合理（工作盒外）—— 当"没有目标"处理
|
||||
|
||||
|
||||
class FeatureJumpGate:
|
||||
"""有状态的跳变确认门限。线程安全由调用方保证（servo 在锁内调用）。"""
|
||||
|
||||
NEAR = "near" # 正常观测：照常滤波更新
|
||||
HOLD = "hold" # 疑似假跳：本次不采信，位置保持不动
|
||||
JUMP = "jump" # 已确认的真实重定位：允许 reset 到新位置
|
||||
|
||||
def __init__(self, jump_m: float, confirm_frames: int = 3) -> None:
|
||||
self.jump_m = float(jump_m)
|
||||
# 至少 1 帧才谈得上"确认"；<=1 就等于退回旧的"单帧采信"，但那是显式选择，
|
||||
# 不是静默降级 —— 这里仍然保留参数以方便现场调节。
|
||||
self.confirm_frames = max(1, int(confirm_frames))
|
||||
self._cand: Optional[np.ndarray] = None
|
||||
self._count = 0
|
||||
# 诊断计数：连续 hold 了多少帧（供日志/排查，见 servo_node 的告警）。
|
||||
self.hold_frames = 0
|
||||
self.hold_total = 0
|
||||
|
||||
def reset(self) -> None:
|
||||
"""清空待确认状态（正常观测、或已确认后都要清）。"""
|
||||
self._cand = None
|
||||
self._count = 0
|
||||
self.hold_frames = 0
|
||||
|
||||
@staticmethod
|
||||
def _dist(a: np.ndarray, b: np.ndarray) -> float:
|
||||
return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
|
||||
|
||||
def update(self, p_xy: np.ndarray, ref_xy: np.ndarray) -> str:
|
||||
"""喂一次观测。
|
||||
|
||||
参数
|
||||
----
|
||||
p_xy : 本帧测到的 XY（base 系，米）
|
||||
ref_xy : 当前滤波后的位置 XY（米）—— 判断"跳没跳"的基准
|
||||
|
||||
返回 :data:`NEAR` / :data:`HOLD` / :data:`JUMP` 之一。
|
||||
"""
|
||||
p = np.asarray(p_xy, dtype=float).reshape(2)
|
||||
ref = np.asarray(ref_xy, dtype=float).reshape(2)
|
||||
if self._dist(p, ref) <= self.jump_m:
|
||||
self.reset()
|
||||
return self.NEAR
|
||||
|
||||
# 大跳：本次观测只算"候选"。候选之间互相对不上就从头再数。
|
||||
if self._cand is None or self._dist(p, self._cand) > self.jump_m:
|
||||
self._cand = p.copy()
|
||||
self._count = 1
|
||||
else:
|
||||
self._count += 1
|
||||
if self._count >= self.confirm_frames:
|
||||
self.reset()
|
||||
return self.JUMP
|
||||
self.hold_frames += 1
|
||||
self.hold_total += 1
|
||||
return self.HOLD
|
||||
@@ -0,0 +1,405 @@
|
||||
"""S00 开机自检：把"能不能跑"这件事变成一张**可打印的清单**（纯逻辑部分）。
|
||||
|
||||
为什么要有这一阶段
|
||||
------------------
|
||||
看板 S00 的要求是"各服务就绪后才开指令窗，任一不就绪即中止并报告"。它挡的是
|
||||
这个仓库最常见的一类事故：**流程跑起来了，只是没接上硬件**。例如：
|
||||
|
||||
* 触觉没起 → 三阶段的力阈值全是 0 → 搜孔"一直没找到"（实际是没数据）；
|
||||
* 相机没起 → 精拍拿不到图 → 报"精拍未拿到图像"（离真正原因差三层）；
|
||||
* 夹爪没起 → S08 松爪/夹紧静默失败 → 棘轮循环空转，看起来像"拧不动"。
|
||||
|
||||
放纯逻辑里的原因
|
||||
----------------
|
||||
"要检查哪些东西"是**事实**，会随现场（两台臂、两种夹爪后端、D435 有没有标定）
|
||||
变化；"怎么检查"是 ROS 机制。前者做成纯函数，就能被测试钉住：
|
||||
|
||||
* 少一项检查 → 测试红（比如"忘了查触觉"）；
|
||||
* 检查项的顺序无关紧要，但**必检项**和**选检项**的划分必须明确 ——
|
||||
把核心判据依赖的触觉标成选检，就会在没传感器时一路跑到力判据才失败。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, Iterable, List, Mapping, Optional, Sequence, Tuple
|
||||
|
||||
#: 探针类型 —— 决定节点用哪种方式确认"就绪"。
|
||||
#:
|
||||
#: ``service`` 服务在线且类型匹配
|
||||
#: ``action`` action server 在线
|
||||
#: ``topic`` 话题在，且**真的收到过消息**（只看 topic list 会假阳性）
|
||||
#: ``tactile`` 触觉状态话题在发，且 ``connected=true``
|
||||
#: ``camera`` 图像话题在发（收到首帧）
|
||||
#: ``detector`` 算法权重能加载（推理预热）
|
||||
PROBES = ("service", "action", "topic", "tactile", "camera", "detector")
|
||||
|
||||
#: ``detector`` 自检除了"权重文件在"，还要求这些 Python 模块能 import。
|
||||
#: 为什么必须查：权重文件在 ≠ 能推理。实测现场 —— 仿真环境（ros_gz_sim）里
|
||||
#: 没有 torch/ultralytics，S00 的"检测算法预热"照样报 OK，然后 S04 空转 150s
|
||||
#: 才报"没锁定"，看起来像算法或标定问题（真正的错在很远的另一处）。
|
||||
#: ``ultralytics`` 会连带把 torch 拉进来，查它就够；真要细分再看 yolo_detector。
|
||||
DETECTOR_MODULES = ("ultralytics",)
|
||||
|
||||
#: 感知来源（``site.yaml`` 顶层 ``perception.source``）—— 谁产生"视觉测量"。
|
||||
#:
|
||||
#: ``yolo`` 本体 perception_node（要 torch/ultralytics）
|
||||
#: ``truth`` 仿真真值：按场景里已知的物体位姿直接算目标点，不跑推理
|
||||
#: ``external`` 外部节点按同一话题（``visual_servo/filtered_point``）提供
|
||||
#:
|
||||
#: ⚠️ 别和 ``yolo.backend`` 混：那个是"**怎么**推理"（tensorrt/onnx）。
|
||||
PERCEPTION_SOURCES = ("yolo", "truth", "external")
|
||||
|
||||
|
||||
def perception_source_of(perception_cfg: Optional[Mapping] = None) -> str:
|
||||
"""读 ``perception.source`` 并校验。**未知值直接报错，不默默当 yolo**。
|
||||
|
||||
默默退回 yolo 的后果：仿真里 perception_node 起不来，S00 又去查
|
||||
"推理依赖"，报出来的错跟真正的原因（source 拼错）隔着两层。
|
||||
"""
|
||||
src = str(((perception_cfg or {}) or {}).get("source") or "yolo").strip().lower()
|
||||
if src not in PERCEPTION_SOURCES:
|
||||
raise ValueError(
|
||||
f"site.yaml 的 perception.source = {src!r} 不认识。"
|
||||
f"只支持 {' / '.join(PERCEPTION_SOURCES)}"
|
||||
"（yolo=本体推理，truth=仿真真值，external=外部节点提供）。"
|
||||
)
|
||||
return src
|
||||
|
||||
|
||||
def missing_modules(modules: Sequence[str] = DETECTOR_MODULES) -> List[str]:
|
||||
"""哪些模块当前解释器里 import 不到（纯逻辑，可离线测试）。"""
|
||||
import importlib.util
|
||||
|
||||
out: List[str] = []
|
||||
for m in modules:
|
||||
try:
|
||||
if importlib.util.find_spec(m) is None:
|
||||
out.append(m)
|
||||
except (ImportError, ValueError):
|
||||
# find_spec 对"父包不存在/名字非法"会抛，这同样等于"用不了"
|
||||
out.append(m)
|
||||
return out
|
||||
|
||||
|
||||
def dependency_advice(missing: Sequence[str]) -> str:
|
||||
"""缺依赖时给**两条**出路：装它、或换掉这个后端。
|
||||
|
||||
只写"请安装 X"是不够的：仿真环境常常是离线的（这台机器实测 pip 直接
|
||||
SSL 失败），那样的话现场需要的是"换一条不依赖它的路"，而不是反复重试安装。
|
||||
"""
|
||||
mods = ", ".join(missing)
|
||||
return (
|
||||
f"缺推理依赖 {mods} —— 权重文件在也没用，导入不了就跑不了推理。两条路：\n"
|
||||
f" 1) 装上：pip install {' '.join(missing)}（需要联网；本机仿真环境离线时做不到）\n"
|
||||
" 2) 换后端：仿真里可以不用 YOLO —— 由外部按同一话题/同一类型提供感知结果\n"
|
||||
" （见 devices/sim/gazebo/README.md 的「仿真里哪些模块用真值替代」一节）"
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class BootCheck:
|
||||
"""一项自检。"""
|
||||
|
||||
key: str
|
||||
"""稳定标识（日志/结果 JSON 里用它，改名要同步文档）。"""
|
||||
|
||||
label: str
|
||||
"""人读的名字。"""
|
||||
|
||||
probe: str
|
||||
"""见 :data:`PROBES`。"""
|
||||
|
||||
target: str
|
||||
"""要探测的对象：服务名 / action 名 / 话题名 / 权重路径。"""
|
||||
|
||||
why: str
|
||||
"""**缺了会怎样**。自检失败的提示必须写这个 —— 只报"未就绪"没人知道该不该急。"""
|
||||
|
||||
required: bool = True
|
||||
"""必检 / 选检。选检失败只告警，不拦流程。"""
|
||||
|
||||
expect_type: Optional[str] = None
|
||||
"""期望的消息/服务类型（有值时类型不符即算失败）。"""
|
||||
|
||||
|
||||
#: 需要"绝对名字"的探针：相对名字会按节点命名空间解析，
|
||||
#: 节点被放进 namespace 之后 ``home`` 会变成 ``/ns/home`` —— 自检就会去探一个
|
||||
#: 不存在的名字，报"未就绪"。统一补前导斜杠，把这件事变成不可能发生。
|
||||
_ABSOLUTE_PROBES = frozenset({"service", "action", "topic", "tactile", "camera"})
|
||||
|
||||
|
||||
def abs_name(name: str) -> str:
|
||||
"""把 ROS 名字规范成绝对名（``home`` → ``/home``）。
|
||||
|
||||
对外公开：调用方（节点）也要用同一套规范化去比较名字，否则会出现
|
||||
"清单里是 /foo、参数里是 foo，比较不相等"这类只在运行时暴露的错。
|
||||
"""
|
||||
n = (name or "").strip()
|
||||
if not n:
|
||||
return n
|
||||
return n if n.startswith("/") else "/" + n
|
||||
|
||||
|
||||
def _topic(prefix: str, suffix: str) -> str:
|
||||
"""拼话题名，容忍 ``prefix`` 带不带前导斜杠。"""
|
||||
p = "/" + prefix.strip("/") if prefix else ""
|
||||
return f"{p}/{suffix.strip('/')}"
|
||||
|
||||
|
||||
def build_checks(
|
||||
*,
|
||||
joint_action: str,
|
||||
camera: str,
|
||||
tactile_topic: str,
|
||||
gripper_open: str,
|
||||
gripper_close: str,
|
||||
gripper_status: str,
|
||||
gripper_status_type: str,
|
||||
d435_camera: Optional[str] = None,
|
||||
tool_name: str = "",
|
||||
weights_path: str = "",
|
||||
perception_source: str = "yolo",
|
||||
) -> List[BootCheck]:
|
||||
"""按现场情况生成检查清单。
|
||||
|
||||
:param joint_action: JTC action 名（``rizon_arm_controller/follow_joint_trajectory``）。
|
||||
:param camera: 腕部相机命名空间（``d405_left``）。
|
||||
:param tactile_topic: 触觉状态话题（``/tactile/left/status``）。
|
||||
:param gripper_status: 夹爪状态话题，随后端不同（``/gripper/status`` 或 ``/gripper/states``）。
|
||||
:param d435_camera: 手外相机命名空间。给 None 表示这台臂没有手外相机 →
|
||||
不生成 S03 相关的检查项（**不生成**比"生成了一个必然失败的项"更诚实：
|
||||
后者会让每次启动都报一条无关的错，很快所有人都不看自检输出了）。
|
||||
:param tool_name: ``site.yaml`` 的 ``robot.tool_name``。空 = 没指定工具系，
|
||||
则 S01 的"工具系垂直向下锁定"无法确认 → 单独列为选检告警。
|
||||
:param weights_path: 检测权重路径。空 = 不检查推理预热。
|
||||
:param perception_source: ``site.yaml`` 的 ``perception.source``。**只有
|
||||
``yolo`` 才查推理预热**：truth/external 根本不跑推理，查了必然是
|
||||
假阴性（"缺 ultralytics"），而那是预期之内的。这里刻意**不生成**
|
||||
这一项，而不是生成一个永远 OK 的项 —— 后者会让人以为"感知也验过了"。
|
||||
|
||||
清单的顺序 = 打印顺序 = 排障顺序：先臂、再夹爪、再相机、再触觉、最后算法。
|
||||
"""
|
||||
checks: List[BootCheck] = [
|
||||
BootCheck(
|
||||
key="arm_jtc",
|
||||
label="机械臂关节轨迹控制器",
|
||||
probe="action",
|
||||
target=joint_action,
|
||||
why="没有它，S01/S06/S07/S10 的所有运动都发不出去",
|
||||
# 类型必须是 **control_msgs**：本流程（和 stage2/stage3/pipeline 一样）
|
||||
# 直接把轨迹发给 ros2_control 的 JTC 服务端，不经过 arm_control 的
|
||||
# 轨迹适配器（那个用的是 arm_msgs/action/FollowJointTrajectory）。
|
||||
# 写成 arm_msgs 的会得到一个"永远探不到服务端"的自检 —— 假阴性，
|
||||
# 而且看起来像控制器没起来。
|
||||
expect_type="control_msgs/action/FollowJointTrajectory",
|
||||
),
|
||||
BootCheck(
|
||||
key="arm_home",
|
||||
label="回零服务",
|
||||
probe="service",
|
||||
target="/home",
|
||||
why="S01 回 home、S11 复位都靠它",
|
||||
expect_type="std_srvs/srv/Trigger",
|
||||
),
|
||||
BootCheck(
|
||||
key="arm_named_pose",
|
||||
label="命名位姿服务",
|
||||
probe="service",
|
||||
target="/set_named_pose",
|
||||
why="/home 内部转发给它；它不在时回零会超时",
|
||||
expect_type="arm_msgs/srv/SetNamedPose",
|
||||
),
|
||||
BootCheck(
|
||||
key="arm_compliant",
|
||||
label="柔顺下压 action",
|
||||
probe="action",
|
||||
target="/compliant_descent",
|
||||
# 选检：S07 现在走节点内的"步进 + 力突变停"（与这个 action 内部
|
||||
# 同一套做法），不依赖它；有它时驱动侧原语更顺，属于加分项。
|
||||
# 标成必检会让仿真（arm_driver_flexiv 不参与）永远过不了自检。
|
||||
why="有它时 S07 用驱动侧柔顺原语；没有则用节点内步进（不影响正确性）",
|
||||
required=False,
|
||||
expect_type="arm_msgs/action/CompliantDescent",
|
||||
),
|
||||
BootCheck(
|
||||
key="gripper_open",
|
||||
label="夹爪张开服务",
|
||||
probe="service",
|
||||
target=gripper_open,
|
||||
why="S08 棘轮的『松爪』一步靠它；失败会让回转把规拧回去",
|
||||
),
|
||||
BootCheck(
|
||||
key="gripper_close",
|
||||
label="夹爪夹紧服务",
|
||||
probe="service",
|
||||
target=gripper_close,
|
||||
why="S08 的『夹紧』一步靠它；失败会让慢转时规打滑（磨螺纹）",
|
||||
),
|
||||
BootCheck(
|
||||
key="camera_color",
|
||||
label="腕部相机彩图",
|
||||
probe="camera",
|
||||
target=_topic(camera, "color/image_raw"),
|
||||
why="S05 精拍定位 k1/k2 靠它；缺了只能靠 S03 粗定位（精度不够）",
|
||||
expect_type="sensor_msgs/msg/Image",
|
||||
),
|
||||
BootCheck(
|
||||
key="camera_info",
|
||||
label="腕部相机内参",
|
||||
probe="topic",
|
||||
target=_topic(camera, "color/camera_info"),
|
||||
why="没有内参就无法把像素反投影成基座坐标（会退化成 60° 默认视场）",
|
||||
expect_type="sensor_msgs/msg/CameraInfo",
|
||||
),
|
||||
BootCheck(
|
||||
key="tactile_status",
|
||||
label="触觉三向力",
|
||||
probe="tactile",
|
||||
target=tactile_topic,
|
||||
why="S07 切向力突变停、S08 拧紧判据、E1 掉件都依赖它；"
|
||||
"缺了这几个判据全部失效（流程能走完但结果不可信）",
|
||||
expect_type="arm_msgs/msg/TactileStatus",
|
||||
),
|
||||
BootCheck(
|
||||
key="gripper_status",
|
||||
label="夹爪状态反馈",
|
||||
probe="topic",
|
||||
target=gripper_status,
|
||||
why="E1 掉件检测靠它；缺了掉件只能靠人看",
|
||||
expect_type=gripper_status_type,
|
||||
),
|
||||
]
|
||||
|
||||
if d435_camera:
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="coarse_camera",
|
||||
label="手外粗定位相机",
|
||||
probe="camera",
|
||||
target=_topic(d435_camera, "color/image_raw"),
|
||||
why="S03 兜底粗定位用；缺了只能在 S04 跟踪上多试几次",
|
||||
required=False,
|
||||
expect_type="sensor_msgs/msg/Image",
|
||||
)
|
||||
)
|
||||
if tool_name:
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="tool_frame",
|
||||
label="工具坐标系已指定",
|
||||
probe="service", # 具体内容由节点按 tool_name 复核（只查服务在不在）
|
||||
target="/switch_tool",
|
||||
why=f"要求 tool1（垂直向下、位姿锁定），site.yaml 里配的是 {tool_name!r}",
|
||||
required=False,
|
||||
expect_type="arm_msgs/srv/SwitchTool",
|
||||
)
|
||||
)
|
||||
# 配了工具系就必须能切：S01 切不过去会直接 TIMEOUT 中止，
|
||||
# 所以在自检里按**必检**拦下（早报比跑到 S01 再报少一次上电动作）。
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="arm_switch_tool",
|
||||
label="工具系切换服务",
|
||||
probe="service",
|
||||
target="/switch_tool",
|
||||
why=f"site.yaml 指定了工具 {tool_name!r}，切不过去后面全是歪的",
|
||||
expect_type="arm_msgs/srv/SwitchTool",
|
||||
)
|
||||
)
|
||||
else:
|
||||
# 没配工具系：S01 只会告警不会切，所以这条只算选检 ——
|
||||
# 真机没配工具系是**配置问题**（看板要求 tool1），但仿真里根本没有
|
||||
# 工具系切换服务，标必检会让仿真永远过不去。
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="arm_switch_tool",
|
||||
label="工具系切换服务",
|
||||
probe="service",
|
||||
target="/switch_tool",
|
||||
why="本机没配 robot.tool_name：真机应配 tool1 并确认这个服务在；仿真不需要",
|
||||
required=False,
|
||||
expect_type="arm_msgs/srv/SwitchTool",
|
||||
)
|
||||
)
|
||||
if weights_path and perception_source == "yolo":
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="detector",
|
||||
label="检测算法预热",
|
||||
probe="detector",
|
||||
target=weights_path,
|
||||
why="S04 跟踪、S03 粗定位都要它；预热失败会在第一次跟踪时才炸",
|
||||
)
|
||||
)
|
||||
|
||||
# 网络端点统一成绝对名（见 _ABSOLUTE_PROBES 的说明）。
|
||||
# detector 的 target 是文件路径，不参与规范化。
|
||||
return [
|
||||
c if c.probe not in _ABSOLUTE_PROBES
|
||||
else BootCheck(
|
||||
key=c.key, label=c.label, probe=c.probe, target=abs_name(c.target),
|
||||
why=c.why, required=c.required, expect_type=c.expect_type,
|
||||
)
|
||||
for c in checks
|
||||
]
|
||||
|
||||
|
||||
def format_report(results: Sequence[Tuple[BootCheck, bool, str]]) -> Tuple[str, List[str]]:
|
||||
"""把逐项结果排成一张表，返回 ``(表文本, 失败项描述)``。
|
||||
|
||||
``results`` 每项是 ``(检查项, 是否就绪, 细节)``。
|
||||
|
||||
两张输出分开是为了让调用方能按"必检失败"决定是否中止：表用来给人看，
|
||||
失败项描述用来进日志/结果 JSON。
|
||||
"""
|
||||
lines: List[str] = ["S00 自检："]
|
||||
failures: List[str] = []
|
||||
for item, ok, detail in results:
|
||||
mark = "OK " if ok else ("缺失" if item.required else "告警")
|
||||
tag = "" if item.required else "（选检）"
|
||||
lines.append(f" [{mark}] {item.label}{tag} {item.target}")
|
||||
if detail:
|
||||
lines.append(f" {detail}")
|
||||
if not ok:
|
||||
prefix = "必检项未就绪" if item.required else "选检项未就绪"
|
||||
failures.append(f"{prefix}：{item.label}（{item.target}）—— {item.why}")
|
||||
if failures:
|
||||
lines.append(f" 共 {len(failures)} 项未就绪")
|
||||
else:
|
||||
lines.append(" 全部就绪，开指令窗")
|
||||
return "\n".join(lines), failures
|
||||
|
||||
|
||||
def blocking_failures(results: Iterable[Tuple[BootCheck, bool, str]]) -> List[str]:
|
||||
"""只挑出**必检且未就绪**的项 —— 决定要不要中止的就是它们。
|
||||
|
||||
和 :func:`format_report` 的 ``failures`` 的区别：那里两项都收（含选检），
|
||||
这里只收必检。合成一个函数的话，调用方一不留神就会拿选检失败去中止流程，
|
||||
于是"D435 没接"变成"装配跑不起来"。
|
||||
"""
|
||||
return [
|
||||
f"必检项未就绪：{item.label}（{item.target}）—— {item.why}"
|
||||
for item, ok, _detail in results
|
||||
if (not ok) and item.required
|
||||
]
|
||||
|
||||
|
||||
def optional_warnings(results: Iterable[Tuple[BootCheck, bool, str]]) -> List[str]:
|
||||
"""选检未就绪的告警（不拦流程，但必须打出来）。"""
|
||||
return [
|
||||
f"选检项未就绪：{item.label}（{item.target}）—— {item.why}"
|
||||
for item, ok, _detail in results
|
||||
if (not ok) and not item.required
|
||||
]
|
||||
|
||||
|
||||
def summary_counts(results: Sequence[Tuple[BootCheck, bool, str]]) -> Dict[str, int]:
|
||||
"""统计``必检/选检``各自通过多少，给启动日志一行摘要。"""
|
||||
out = {"required_ok": 0, "required_total": 0, "optional_ok": 0, "optional_total": 0}
|
||||
for item, ok, _detail in results:
|
||||
slot = "required" if item.required else "optional"
|
||||
out[f"{slot}_total"] += 1
|
||||
if ok:
|
||||
out[f"{slot}_ok"] += 1
|
||||
return out
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,178 @@
|
||||
"""通止规通/不通：只看传感器切向方向，外加一个结束通知。
|
||||
|
||||
口径
|
||||
----
|
||||
切向方向是传感器平面上的角，不是关节力矩::
|
||||
|
||||
atan2(fsheary, fshearx)，单位度，范围 (-180, 180]，0° 指向 +fshearx，逆时针为正。
|
||||
|
||||
这是全仓库唯一一份算法。画面工具 ``tools/ctag_sensor_view`` 只调用这里，
|
||||
不许再写一份 ``atan2``。
|
||||
|
||||
判定
|
||||
----
|
||||
- 方向相对一开始的读数转过 ``start_deg``：开始拧，结论变成「正在拧」。
|
||||
- 正在拧的时候方向停住（连续 ``stop_hold_s`` 秒转角不超过 ``stop_delta_deg``），
|
||||
并且还没有结束通知：不通。
|
||||
- ``notify_finished()`` 是结束通知的入口。正在拧时收到，结论变成通。
|
||||
通知来得比停更早，会先记下，等它停住再判通。
|
||||
- 还没开始拧就收到通知，不判通。已经判成不通之后，迟到的通知不翻案。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Optional
|
||||
|
||||
# 结论。界面和调用方都用这四个字符串，不要另起一套。
|
||||
PASS_IDLE = "idle" # 方向还没变
|
||||
PASS_TURNING = "turning" # 方向在变，已经开始拧
|
||||
PASS_OK = "ok" # 通
|
||||
PASS_NG = "ng" # 不通
|
||||
|
||||
PASS_LABEL = {
|
||||
PASS_IDLE: "待拧",
|
||||
PASS_TURNING: "正在拧",
|
||||
PASS_OK: "通",
|
||||
PASS_NG: "不通",
|
||||
}
|
||||
|
||||
# 形变读数上的第一档门槛。切向太小的时候角度没有意义，不能拿来当“方向变了”。
|
||||
MIN_SHEAR: float = 0.05
|
||||
# 相对起始方向转过这么多度，才算开始拧。几度的抖动留在待拧。
|
||||
START_DEG: float = 12.0
|
||||
# 正在拧时，相邻有效读数的转角小于这个值，这一拍算停着。
|
||||
STOP_DELTA_DEG: float = 3.0
|
||||
# 连续这么久都停着，才收下“停住了”。单拍抖一下不算停。
|
||||
STOP_HOLD_S: float = 0.5
|
||||
|
||||
|
||||
def shear_direction(fshearx: float, fsheary: float) -> tuple[float, float]:
|
||||
"""返回 ``(角度°, 切向大小)``。切向为 0 时角度记 0，表示这次没有方向。"""
|
||||
x = float(fshearx)
|
||||
y = float(fsheary)
|
||||
magnitude = math.hypot(x, y)
|
||||
if magnitude == 0.0:
|
||||
return 0.0, 0.0
|
||||
angle = math.degrees(math.atan2(y, x))
|
||||
if angle <= -180.0:
|
||||
angle += 360.0
|
||||
return angle, magnitude
|
||||
|
||||
|
||||
def angle_delta_deg(start_deg: float, now_deg: float) -> float:
|
||||
"""从 ``start_deg`` 转到 ``now_deg`` 的最短角度，范围 ``(-180, 180]``。
|
||||
|
||||
179° 到 -179° 只有 2°，不能按 358° 去算。差一整圈时收成 +180°。
|
||||
"""
|
||||
delta = (float(now_deg) - float(start_deg) + 180.0) % 360.0 - 180.0
|
||||
if delta <= -180.0:
|
||||
delta += 360.0
|
||||
return delta
|
||||
|
||||
|
||||
class GaugePassJudge:
|
||||
"""吃一串方向读数，再加上一次结束通知，给出通或不通。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
start_deg: float = START_DEG,
|
||||
stop_delta_deg: float = STOP_DELTA_DEG,
|
||||
stop_hold_s: float = STOP_HOLD_S,
|
||||
min_shear: float = MIN_SHEAR,
|
||||
) -> None:
|
||||
if start_deg <= 0.0 or stop_delta_deg < 0.0 or stop_hold_s < 0.0 or min_shear < 0.0:
|
||||
raise ValueError("方向门槛必须是非负的，开始拧的转角必须为正")
|
||||
if stop_delta_deg >= start_deg:
|
||||
raise ValueError("停住的转角门槛必须小于开始拧的转角，否则一动就会被当成停")
|
||||
self.start_deg = float(start_deg)
|
||||
self.stop_delta_deg = float(stop_delta_deg)
|
||||
self.stop_hold_s = float(stop_hold_s)
|
||||
self.min_shear = float(min_shear)
|
||||
self._verdict = PASS_IDLE
|
||||
self._notified = False
|
||||
self._ref_angle: Optional[float] = None
|
||||
self._last_angle: Optional[float] = None
|
||||
self._last_move_s: Optional[float] = None
|
||||
self._angle: Optional[float] = None
|
||||
self._magnitude = 0.0
|
||||
|
||||
@property
|
||||
def verdict(self) -> str:
|
||||
return self._verdict
|
||||
|
||||
def snapshot(self) -> dict:
|
||||
if self._verdict == PASS_OK:
|
||||
reason = "收到结束通知"
|
||||
elif self._verdict == PASS_NG:
|
||||
reason = "方向停了，没有结束通知"
|
||||
elif self._verdict == PASS_TURNING:
|
||||
reason = "方向在变，开始拧了"
|
||||
else:
|
||||
reason = "方向还没变"
|
||||
return {
|
||||
"verdict": self._verdict,
|
||||
"label": PASS_LABEL[self._verdict],
|
||||
"reason": reason,
|
||||
"notified": self._notified,
|
||||
"angle_deg": self._angle,
|
||||
"magnitude": self._magnitude,
|
||||
}
|
||||
|
||||
def reset(self) -> None:
|
||||
self._verdict = PASS_IDLE
|
||||
self._notified = False
|
||||
self._ref_angle = None
|
||||
self._last_angle = None
|
||||
self._last_move_s = None
|
||||
self._angle = None
|
||||
self._magnitude = 0.0
|
||||
|
||||
def notify_finished(self, now_s: Optional[float] = None) -> str:
|
||||
"""结束通知。对面工位或上游流程结束时调用，目前没有接外部信号。
|
||||
|
||||
``now_s`` 只留给和 ``update`` 对齐的时钟，通知本身不靠它改角度。
|
||||
"""
|
||||
del now_s
|
||||
self._notified = True
|
||||
if self._verdict == PASS_TURNING:
|
||||
self._verdict = PASS_OK
|
||||
return self._verdict
|
||||
|
||||
def update(self, angle_deg: float, magnitude: float, now_s: float) -> str:
|
||||
"""喂一帧方向。已经是通或不通之后，后面的读数不再改结论。"""
|
||||
if self._verdict in (PASS_OK, PASS_NG):
|
||||
return self._verdict
|
||||
now = float(now_s)
|
||||
mag = float(magnitude)
|
||||
self._magnitude = mag
|
||||
reliable = mag >= self.min_shear
|
||||
if self._verdict == PASS_IDLE:
|
||||
if not reliable:
|
||||
return PASS_IDLE
|
||||
angle = float(angle_deg)
|
||||
self._angle = angle
|
||||
if self._ref_angle is None:
|
||||
self._ref_angle = angle
|
||||
return PASS_IDLE
|
||||
if abs(angle_delta_deg(self._ref_angle, angle)) >= self.start_deg:
|
||||
self._begin(angle, now)
|
||||
return self._verdict
|
||||
if reliable:
|
||||
angle = float(angle_deg)
|
||||
self._angle = angle
|
||||
assert self._last_angle is not None
|
||||
if abs(angle_delta_deg(self._last_angle, angle)) >= self.stop_delta_deg:
|
||||
self._last_angle = angle
|
||||
self._last_move_s = now
|
||||
return PASS_TURNING
|
||||
if self._last_move_s is not None and now - self._last_move_s >= self.stop_hold_s:
|
||||
self._verdict = PASS_OK if self._notified else PASS_NG
|
||||
return self._verdict
|
||||
|
||||
def _begin(self, angle: float, now_s: float) -> None:
|
||||
self._verdict = PASS_TURNING
|
||||
self._last_angle = angle
|
||||
self._last_move_s = now_s
|
||||
self._angle = angle
|
||||
@@ -0,0 +1,40 @@
|
||||
#!/usr/bin/env python3
|
||||
"""向后兼容 shim —— 实现已移到 :mod:`arm_kinematics.geom_ik`。
|
||||
|
||||
为什么搬走
|
||||
----------
|
||||
这套几何雅可比 + DLS 逆解同时被应用层（视觉伺服、搜孔装配）和驱动层
|
||||
（``arm_driver_flexiv`` 的柔顺下压）需要。留在 ``arm_application`` 里会让驱动层
|
||||
反过来依赖应用层，属于层次倒置。搬到不依赖 ROS 的 ``arm_kinematics`` 后，
|
||||
两边都依赖一个底层包。
|
||||
|
||||
为什么保留这个文件
|
||||
------------------
|
||||
``stage2_locate_node`` / ``stage3_search_node`` / ``servo_node`` 等现有代码都在
|
||||
``from arm_application.geom_ik import ...``。保留 shim 就不用改动这些已经在跑的
|
||||
文件 —— 重构不应牵连能工作的代码。
|
||||
|
||||
新代码请直接 ``from arm_kinematics import dls_twist``。
|
||||
"""
|
||||
|
||||
from arm_kinematics.geom_ik import ( # noqa: F401
|
||||
RIZON_LINK_AXES,
|
||||
dls_position_step,
|
||||
dls_translate_hold_ori,
|
||||
dls_twist,
|
||||
geometric_jacobian,
|
||||
load_handeye_T,
|
||||
position_jacobian,
|
||||
quat_xyzw_to_R,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"RIZON_LINK_AXES",
|
||||
"quat_xyzw_to_R",
|
||||
"geometric_jacobian",
|
||||
"position_jacobian",
|
||||
"dls_position_step",
|
||||
"dls_twist",
|
||||
"dls_translate_hold_ori",
|
||||
"load_handeye_T",
|
||||
]
|
||||
@@ -0,0 +1,404 @@
|
||||
#!/usr/bin/env python3
|
||||
"""手眼标定辅助工具：棋盘稳定判定 + AX=XB 求解。
|
||||
|
||||
有 ``cv2.calibrateHandEye`` 时用它，否则用 Tsai-Lenz（NumPy 实现）。
|
||||
棋盘检测始终依赖 OpenCV。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Deque, List, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
import cv2
|
||||
except Exception: # noqa: BLE001
|
||||
cv2 = None # type: ignore
|
||||
|
||||
|
||||
@dataclass
|
||||
class BoardConfig:
|
||||
cols: int # 内角点数（宽）
|
||||
rows: int # 内角点数（高）
|
||||
square_size: float # 米
|
||||
|
||||
|
||||
@dataclass
|
||||
class CameraIntrinsics:
|
||||
K: np.ndarray
|
||||
dist: np.ndarray
|
||||
|
||||
|
||||
def R_to_quat_xyzw(R: np.ndarray) -> Tuple[float, float, float, float]:
|
||||
"""3×3 旋转 → ``(x, y, z, w)`` 四元数（Shepperd 法，按最大分量选分支）。
|
||||
|
||||
为什么放在库里而不是各个 launch 里：这段换算**已经在两份 launch 里各抄了
|
||||
一遍**，而且抄错过（对角分支的公式被写歪，症状是手眼 TF 差了 30°，
|
||||
定位看起来"总能到附近"但永远差一点）。换算只有一份才可能被测试：
|
||||
``test_handeye_lib_roundtrip.py`` 拿真实标定文件验证
|
||||
``quat_xyzw_to_R(R_to_quat_xyzw(R)) == R``。
|
||||
"""
|
||||
R = np.asarray(R, dtype=float).reshape(3, 3)
|
||||
tr = float(R[0, 0] + R[1, 1] + R[2, 2])
|
||||
if tr > 0.0:
|
||||
s = math.sqrt(tr + 1.0) * 2.0
|
||||
w = 0.25 * s
|
||||
x = (R[2, 1] - R[1, 2]) / s
|
||||
y = (R[0, 2] - R[2, 0]) / s
|
||||
z = (R[1, 0] - R[0, 1]) / s
|
||||
elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
|
||||
s = math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
|
||||
w = (R[2, 1] - R[1, 2]) / s
|
||||
x = 0.25 * s
|
||||
y = (R[0, 1] + R[1, 0]) / s
|
||||
z = (R[0, 2] + R[2, 0]) / s
|
||||
elif R[1, 1] > R[2, 2]:
|
||||
s = math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
|
||||
w = (R[0, 2] - R[2, 0]) / s
|
||||
x = (R[0, 1] + R[1, 0]) / s
|
||||
y = 0.25 * s
|
||||
z = (R[1, 2] + R[2, 1]) / s
|
||||
else:
|
||||
s = math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
|
||||
w = (R[1, 0] - R[0, 1]) / s
|
||||
x = (R[0, 2] + R[2, 0]) / s
|
||||
y = (R[1, 2] + R[2, 1]) / s
|
||||
z = 0.25 * s
|
||||
n = math.sqrt(x * x + y * y + z * z + w * w)
|
||||
if n <= 0.0:
|
||||
raise ValueError("旋转矩阵无法转成四元数（矩阵为零？）")
|
||||
return x / n, y / n, z / n, w / n
|
||||
|
||||
|
||||
def read_transform_yaml(path: str | Path) -> Tuple[np.ndarray, Tuple[float, float, float, float]]:
|
||||
"""读手眼标定产物 → ``(xyz_m, quat_xyzw)``。
|
||||
|
||||
文件格式见 :func:`format_transform_yaml`：``T_4x4`` 是 4 行 4 列的列表。
|
||||
读取时**必须校验形状**：手眼文件是现场人手拷来拷去的，形状不对时
|
||||
直接报错，比"取到一堆 nan 然后发了个全零 TF"好得多（后者会让定位
|
||||
永远差一个常量，而且看起来像相机标定不准）。
|
||||
"""
|
||||
import yaml # 局部导入：本模块其它函数不需要 yaml（标定计算本身不依赖）
|
||||
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f) or {}
|
||||
T = data.get("T_4x4")
|
||||
if T is None:
|
||||
raise ValueError(f"{path}: 缺少 T_4x4 字段")
|
||||
arr = np.asarray(T, dtype=float)
|
||||
if arr.shape != (4, 4):
|
||||
raise ValueError(f"{path}: T_4x4 应为 4x4，实际 {arr.shape}")
|
||||
xyz = np.array([arr[0, 3], arr[1, 3], arr[2, 3]], dtype=float)
|
||||
return xyz, R_to_quat_xyzw(arr[:3, :3])
|
||||
|
||||
|
||||
def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray:
|
||||
x, y, z, w = [float(v) for v in q]
|
||||
n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12
|
||||
x, y, z, w = x / n, y / n, z / n, w / n
|
||||
return np.array(
|
||||
[
|
||||
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
|
||||
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
|
||||
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
|
||||
|
||||
def pose_to_Rt(
|
||||
xyz: Sequence[float], quat_xyzw: Sequence[float]
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
R = quat_xyzw_to_R(quat_xyzw)
|
||||
t = np.array([[float(xyz[0])], [float(xyz[1])], [float(xyz[2])]], dtype=float)
|
||||
return R, t
|
||||
|
||||
|
||||
def Rt_to_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = R
|
||||
T[:3, 3] = t.reshape(3)
|
||||
return T
|
||||
|
||||
|
||||
def cam_to_base_T(
|
||||
T_handeye: np.ndarray, T_ee: np.ndarray, mode: str
|
||||
) -> np.ndarray:
|
||||
"""相机系 → 基座系的 4×4。
|
||||
|
||||
* ``eye_in_hand``：``T_handeye`` 是 cam→法兰，再左乘 ``T_ee``（base→法兰）。
|
||||
* ``eye_to_hand``：``T_handeye`` **已经是** cam→基座。再乘 ``T_ee`` 会把
|
||||
固定相机错挂到法兰上 —— 定位总能到附近、永远差一截，看起来像标定不准。
|
||||
"""
|
||||
T_he = np.asarray(T_handeye, dtype=float).reshape(4, 4)
|
||||
tag = str(mode or "").strip().lower()
|
||||
if tag == "eye_to_hand":
|
||||
return T_he
|
||||
if tag != "eye_in_hand":
|
||||
raise ValueError(
|
||||
f"手眼模式只能是 eye_in_hand 或 eye_to_hand，收到 {mode!r}"
|
||||
)
|
||||
return np.asarray(T_ee, dtype=float).reshape(4, 4) @ T_he
|
||||
|
||||
|
||||
def cam_point_to_base(
|
||||
p_cam: np.ndarray,
|
||||
T_handeye: np.ndarray,
|
||||
T_ee: np.ndarray,
|
||||
mode: str,
|
||||
) -> np.ndarray:
|
||||
"""工件三维点（相机系）→ 基座系 XYZ。"""
|
||||
T_bc = cam_to_base_T(T_handeye, T_ee, mode)
|
||||
xyz = np.asarray(p_cam, dtype=float).reshape(3)
|
||||
ph = np.array([float(xyz[0]), float(xyz[1]), float(xyz[2]), 1.0], dtype=float)
|
||||
return (T_bc @ ph)[:3]
|
||||
|
||||
|
||||
def invert_Rt(R: np.ndarray, t: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
Ri = R.T
|
||||
ti = -Ri @ t.reshape(3, 1)
|
||||
return Ri, ti
|
||||
|
||||
|
||||
def board_object_points(cfg: BoardConfig) -> np.ndarray:
|
||||
obj = np.zeros((cfg.rows * cfg.cols, 3), np.float32)
|
||||
xs, ys = np.meshgrid(np.arange(cfg.cols), np.arange(cfg.rows))
|
||||
obj[:, 0] = xs.reshape(-1) * cfg.square_size
|
||||
obj[:, 1] = ys.reshape(-1) * cfg.square_size
|
||||
return obj
|
||||
|
||||
|
||||
def detect_chessboard(
|
||||
image_bgr: np.ndarray, cfg: BoardConfig
|
||||
) -> Optional[Tuple[np.ndarray, Tuple[int, int]]]:
|
||||
"""返回 (角点, pattern_size) 或 None。pattern 为实际使用的 (cols, rows)。"""
|
||||
if cv2 is None:
|
||||
raise RuntimeError('棋盘检测需要 OpenCV（cv2）')
|
||||
gray = cv2.cvtColor(image_bgr, cv2.COLOR_BGR2GRAY)
|
||||
pattern = (int(cfg.cols), int(cfg.rows))
|
||||
flags = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE
|
||||
ok, corners = cv2.findChessboardCorners(gray, pattern, flags)
|
||||
used = pattern
|
||||
if not ok:
|
||||
ok, corners = cv2.findChessboardCorners(
|
||||
gray, (pattern[1], pattern[0]), flags
|
||||
)
|
||||
used = (pattern[1], pattern[0])
|
||||
if not ok or corners is None:
|
||||
return None
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 40, 1e-3)
|
||||
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
|
||||
return np.asarray(corners, dtype=np.float32), used
|
||||
|
||||
|
||||
def solve_board_pose(
|
||||
corners: np.ndarray,
|
||||
cfg: BoardConfig,
|
||||
cam: CameraIntrinsics,
|
||||
pattern: Optional[Tuple[int, int]] = None,
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
if cv2 is None:
|
||||
raise RuntimeError('solvePnP 需要 OpenCV（cv2）')
|
||||
if pattern is None:
|
||||
pattern = (cfg.cols, cfg.rows)
|
||||
board = BoardConfig(pattern[0], pattern[1], cfg.square_size)
|
||||
obj = board_object_points(board)
|
||||
ok, rvec, tvec = cv2.solvePnP(
|
||||
obj, corners, cam.K, cam.dist, flags=cv2.SOLVEPNP_ITERATIVE
|
||||
)
|
||||
if not ok:
|
||||
raise RuntimeError('solvePnP 失败')
|
||||
R, _ = cv2.Rodrigues(rvec)
|
||||
return R.astype(float), np.asarray(tvec, dtype=float).reshape(3, 1)
|
||||
|
||||
|
||||
class ChessboardStabilizer:
|
||||
def __init__(self, window: int = 8, max_pixel_std: float = 0.45) -> None:
|
||||
self.window = int(window)
|
||||
self.max_pixel_std = float(max_pixel_std)
|
||||
self._buf: Deque[np.ndarray] = deque(maxlen=self.window)
|
||||
|
||||
def reset(self) -> None:
|
||||
self._buf.clear()
|
||||
|
||||
def push(self, corners: Optional[np.ndarray]) -> Tuple[bool, float]:
|
||||
if corners is None:
|
||||
self._buf.clear()
|
||||
return False, float('inf')
|
||||
self._buf.append(corners.reshape(-1, 2).astype(float))
|
||||
if len(self._buf) < self.window:
|
||||
return False, float('inf')
|
||||
stack = np.stack(list(self._buf), axis=0)
|
||||
std = float(np.mean(np.std(stack, axis=0)))
|
||||
return std <= self.max_pixel_std, std
|
||||
|
||||
|
||||
def _skew(v: np.ndarray) -> np.ndarray:
|
||||
x, y, z = v.reshape(3)
|
||||
return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]])
|
||||
|
||||
|
||||
def _mat_log(R: np.ndarray) -> np.ndarray:
|
||||
c = float(np.clip((np.trace(R) - 1.0) * 0.5, -1.0, 1.0))
|
||||
theta = float(np.arccos(c))
|
||||
if theta < 1e-8:
|
||||
return np.zeros(3)
|
||||
w = np.array(
|
||||
[R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]], dtype=float
|
||||
)
|
||||
return w * (theta / (2.0 * np.sin(theta)))
|
||||
|
||||
|
||||
def _rodrigues(rvec: np.ndarray) -> np.ndarray:
|
||||
r = rvec.reshape(3)
|
||||
theta = float(np.linalg.norm(r))
|
||||
if theta < 1e-12:
|
||||
return np.eye(3)
|
||||
k = r / theta
|
||||
K = _skew(k)
|
||||
return np.eye(3) + np.sin(theta) * K + (1.0 - np.cos(theta)) * (K @ K)
|
||||
|
||||
|
||||
def calibrate_hand_eye_tsai(
|
||||
R_gripper2base: List[np.ndarray],
|
||||
t_gripper2base: List[np.ndarray],
|
||||
R_target2cam: List[np.ndarray],
|
||||
t_target2cam: List[np.ndarray],
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""Tsai-Lenz 方法。返回 R_cam2gripper、t_cam2gripper。"""
|
||||
n = len(R_gripper2base)
|
||||
if n < 3:
|
||||
raise ValueError('至少需要 3 个位姿')
|
||||
|
||||
Hg = [Rt_to_T(R, t) for R, t in zip(R_gripper2base, t_gripper2base)]
|
||||
Hc = [Rt_to_T(R, t) for R, t in zip(R_target2cam, t_target2cam)]
|
||||
|
||||
A_rows: List[np.ndarray] = []
|
||||
b_rows: List[np.ndarray] = []
|
||||
for i in range(n - 1):
|
||||
A = np.linalg.inv(Hg[i + 1]) @ Hg[i]
|
||||
B = Hc[i + 1] @ np.linalg.inv(Hc[i])
|
||||
ra = _mat_log(A[:3, :3])
|
||||
rb = _mat_log(B[:3, :3])
|
||||
A_rows.append(_skew(ra + rb))
|
||||
b_rows.append(rb - ra)
|
||||
A_mat = np.vstack(A_rows)
|
||||
b_vec = np.concatenate(b_rows)
|
||||
p, _, _, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None)
|
||||
denom = np.sqrt(1.0 + float(p @ p))
|
||||
P = 2.0 * p / denom
|
||||
Rx = _rodrigues(P)
|
||||
|
||||
C_rows: List[np.ndarray] = []
|
||||
d_rows: List[np.ndarray] = []
|
||||
for i in range(n - 1):
|
||||
A = np.linalg.inv(Hg[i + 1]) @ Hg[i]
|
||||
B = Hc[i + 1] @ np.linalg.inv(Hc[i])
|
||||
Ra, ta = A[:3, :3], A[:3, 3:4]
|
||||
_Rb, tb = B[:3, :3], B[:3, 3:4]
|
||||
C_rows.append(Ra - np.eye(3))
|
||||
d_rows.append(Rx @ tb - ta)
|
||||
C = np.vstack(C_rows)
|
||||
d = np.vstack(d_rows)
|
||||
tx, _, _, _ = np.linalg.lstsq(C, d, rcond=None)
|
||||
return Rx.astype(float), tx.reshape(3, 1).astype(float)
|
||||
|
||||
|
||||
def calibrate_hand_eye(
|
||||
R_gripper2base: List[np.ndarray],
|
||||
t_gripper2base: List[np.ndarray],
|
||||
R_target2cam: List[np.ndarray],
|
||||
t_target2cam: List[np.ndarray],
|
||||
*,
|
||||
eye_to_hand: bool = False,
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""
|
||||
eye_in_hand -> X = cam -> gripper（相机装在法兰上）
|
||||
eye_to_hand -> X = cam -> base（相机固定安装）
|
||||
"""
|
||||
Rg = list(R_gripper2base)
|
||||
tg = list(t_gripper2base)
|
||||
if eye_to_hand:
|
||||
Rg = []
|
||||
tg = []
|
||||
for R, t in zip(R_gripper2base, t_gripper2base):
|
||||
Ri, ti = invert_Rt(R, t)
|
||||
Rg.append(Ri)
|
||||
tg.append(ti)
|
||||
|
||||
if cv2 is not None and hasattr(cv2, 'calibrateHandEye'):
|
||||
try:
|
||||
R, t = cv2.calibrateHandEye(
|
||||
Rg, tg, R_target2cam, t_target2cam, method=cv2.CALIB_HAND_EYE_TSAI
|
||||
)
|
||||
return np.asarray(R, float), np.asarray(t, float).reshape(3, 1)
|
||||
except Exception:
|
||||
pass # 退回 NumPy 版 Tsai
|
||||
return calibrate_hand_eye_tsai(Rg, tg, R_target2cam, t_target2cam)
|
||||
|
||||
|
||||
def T_from_flexiv_tcp(xyz_m, qwxyz) -> np.ndarray:
|
||||
"""Studio ``tcp_location`` → 法兰→工具 4×4。
|
||||
|
||||
Flexiv 顺序是 ``[x, y, z, q_w, q_x, q_y, q_z]``，四元数是 wxyz，
|
||||
和本仓库其余地方的 xyzw 不一样。只在这里换一次。
|
||||
"""
|
||||
xyz = np.asarray(xyz_m, dtype=float).reshape(3)
|
||||
q = np.asarray(qwxyz, dtype=float).reshape(7) if np.asarray(qwxyz).size == 7 else np.asarray(qwxyz, dtype=float).reshape(4)
|
||||
if q.size == 7:
|
||||
qw, qx, qy, qz = (float(q[3]), float(q[4]), float(q[5]), float(q[6]))
|
||||
else:
|
||||
qw, qx, qy, qz = (float(q[0]), float(q[1]), float(q[2]), float(q[3]))
|
||||
R = quat_xyzw_to_R((qx, qy, qz, qw))
|
||||
return Rt_to_T(R, xyz.reshape(3, 1))
|
||||
|
||||
|
||||
def cam_to_flange_via_tool(T_cam_to_tool: np.ndarray, T_flange_to_tool: np.ndarray) -> np.ndarray:
|
||||
"""AX=XB 在工具 TCP 上解出 cam→工具后，再折成 cam→法兰。
|
||||
|
||||
``T_flange_to_tool`` 是 Studio TCP：``p_法兰 = T_flange_to_tool · p_工具``。
|
||||
OpenCV 的 ``T_cam_to_tool`` 是 ``p_工具 = T_cam_to_tool · p_相机``。
|
||||
所以 ``T_cam_to_flange = T_flange_to_tool · T_cam_to_tool``。
|
||||
|
||||
乘反了（``T_cam_to_tool · inv(T_ft)``）旋转看着还像，平移会把相机
|
||||
扔到法兰后方约 30 cm。忘了乘则 ``T_4x4`` 停在 TCP 上，沿工具偏 ~225 mm。
|
||||
两种都不报错。
|
||||
"""
|
||||
T_ct = np.asarray(T_cam_to_tool, dtype=float).reshape(4, 4)
|
||||
T_ft = np.asarray(T_flange_to_tool, dtype=float).reshape(4, 4)
|
||||
return T_ft @ T_ct
|
||||
|
||||
|
||||
def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
|
||||
T = Rt_to_T(R, t)
|
||||
lines = [
|
||||
f'# {name}',
|
||||
f'translation_m: [{t[0, 0]:.8f}, {t[1, 0]:.8f}, {t[2, 0]:.8f}]',
|
||||
'T_4x4:',
|
||||
]
|
||||
for row in T:
|
||||
lines.append(' - [' + ', '.join(f'{v:.8f}' for v in row) + ']')
|
||||
return '\n'.join(lines) + '\n'
|
||||
|
||||
|
||||
def format_matrix_yaml(T: np.ndarray, key: str) -> str:
|
||||
arr = np.asarray(T, dtype=float).reshape(4, 4)
|
||||
lines = [f'{key}:']
|
||||
for row in arr:
|
||||
lines.append(' - [' + ', '.join(f'{v:.8f}' for v in row) + ']')
|
||||
return '\n'.join(lines) + '\n'
|
||||
|
||||
|
||||
def is_pose_filled(joints: Sequence[float]) -> bool:
|
||||
"""若全为零 / NaN 占位则返回 False。"""
|
||||
arr = np.asarray(joints, dtype=float)
|
||||
if arr.size != 7:
|
||||
return False
|
||||
if np.any(np.isnan(arr)):
|
||||
return False
|
||||
return float(np.linalg.norm(arr)) > 1e-6
|
||||
@@ -0,0 +1,251 @@
|
||||
#!/usr/bin/env python3
|
||||
"""通过 pyrealsense2 查询 RealSense（D405 / D435）的出厂彩色内参。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import List, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class RealsenseIntrinsics:
|
||||
model: str
|
||||
serial: str
|
||||
width: int
|
||||
height: int
|
||||
fx: float
|
||||
fy: float
|
||||
cx: float
|
||||
cy: float
|
||||
dist: List[float] # OpenCV 顺序，长度 5
|
||||
|
||||
def as_K(self) -> np.ndarray:
|
||||
return np.array(
|
||||
[[self.fx, 0.0, self.cx], [0.0, self.fy, self.cy], [0.0, 0.0, 1.0]],
|
||||
dtype=float,
|
||||
)
|
||||
|
||||
def as_dist(self) -> np.ndarray:
|
||||
d = list(self.dist) + [0.0] * 5
|
||||
return np.array(d[:5], dtype=float).reshape(-1, 1)
|
||||
|
||||
|
||||
# camera_source 为 d405/d435 时的默认 ROS 话题命名空间。
|
||||
REALSENSE_TOPIC_DEFAULTS = {
|
||||
'd405': {
|
||||
'image_topic': '/d405/color/image_raw',
|
||||
'camera_info_topic': '/d405/color/camera_info',
|
||||
},
|
||||
'd435': {
|
||||
'image_topic': '/d435/color/image_raw',
|
||||
'camera_info_topic': '/d435/color/camera_info',
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def _norm_model(model: str) -> str:
|
||||
m = model.strip().lower().replace(' ', '').replace('-', '')
|
||||
if m.startswith('d405') or m in ('realsensed405', 'intelrealsensed405'):
|
||||
return 'd405'
|
||||
if m.startswith('d435') or m in ('realsensed435', 'intelrealsensed435'):
|
||||
return 'd435'
|
||||
return m
|
||||
|
||||
|
||||
def list_realsense_devices() -> List[Tuple[str, str]]:
|
||||
"""返回 [(name, serial), ...]。"""
|
||||
try:
|
||||
import pyrealsense2 as rs
|
||||
except Exception as exc: # noqa: BLE001
|
||||
raise RuntimeError(
|
||||
'未安装 pyrealsense2。'
|
||||
'请在 ros_flexiv 环境中安装：pip/conda install pyrealsense2'
|
||||
) from exc
|
||||
out: List[Tuple[str, str]] = []
|
||||
for d in rs.context().devices:
|
||||
name = d.get_info(rs.camera_info.name)
|
||||
serial = d.get_info(rs.camera_info.serial_number)
|
||||
out.append((name, serial))
|
||||
return out
|
||||
|
||||
|
||||
def query_realsense_color_intrinsics(
|
||||
model: str,
|
||||
*,
|
||||
serial: str = '',
|
||||
width: int = 1280,
|
||||
height: int = 720,
|
||||
) -> RealsenseIntrinsics:
|
||||
"""
|
||||
在给定分辨率下读取 D405/D435 设备上的彩色内参。
|
||||
|
||||
不会启动流管道（只读取 profile 元数据）。
|
||||
"""
|
||||
try:
|
||||
import pyrealsense2 as rs
|
||||
except Exception as exc: # noqa: BLE001
|
||||
raise RuntimeError(
|
||||
'未安装 pyrealsense2。'
|
||||
'请在 ros_flexiv 环境中安装：pip/conda install pyrealsense2'
|
||||
) from exc
|
||||
|
||||
want = _norm_model(model)
|
||||
if want not in ('d405', 'd435'):
|
||||
raise ValueError(f'不支持的型号 {model!r}；请使用 d405 或 d435')
|
||||
|
||||
hint = want.upper() # D405 / D435
|
||||
serial = (serial or '').strip()
|
||||
devices = list(rs.context().devices)
|
||||
if not devices:
|
||||
raise RuntimeError('未连接任何 RealSense 设备')
|
||||
|
||||
chosen = None
|
||||
for d in devices:
|
||||
name = d.get_info(rs.camera_info.name)
|
||||
sn = d.get_info(rs.camera_info.serial_number)
|
||||
if serial and sn != serial:
|
||||
continue
|
||||
name_u = name.upper().replace(' ', '')
|
||||
if hint in name_u or hint in name.upper():
|
||||
chosen = d
|
||||
break
|
||||
|
||||
if chosen is None:
|
||||
avail = ', '.join(
|
||||
f'{d.get_info(rs.camera_info.name)}'
|
||||
f'({d.get_info(rs.camera_info.serial_number)})'
|
||||
for d in devices
|
||||
)
|
||||
raise RuntimeError(
|
||||
f'没有匹配 model={want!r} serial={serial!r} 的 RealSense。'
|
||||
f'已连接的设备：{avail or "无"}'
|
||||
)
|
||||
|
||||
name = chosen.get_info(rs.camera_info.name)
|
||||
sn = chosen.get_info(rs.camera_info.serial_number)
|
||||
|
||||
best = None # (评分, width, height, fx, fy, cx, cy, 畸变系数)
|
||||
for s in chosen.sensors:
|
||||
for p in s.profiles:
|
||||
if p.stream_type() != rs.stream.color:
|
||||
continue
|
||||
vp = p.as_video_stream_profile()
|
||||
try:
|
||||
intr = vp.get_intrinsics()
|
||||
except Exception: # noqa: BLE001
|
||||
continue
|
||||
w, h = int(intr.width), int(intr.height)
|
||||
if w == int(width) and h == int(height):
|
||||
score = 0
|
||||
else:
|
||||
score = abs(w * h - int(width) * int(height)) + 1
|
||||
cand = (
|
||||
score,
|
||||
w,
|
||||
h,
|
||||
float(intr.fx),
|
||||
float(intr.fy),
|
||||
float(intr.ppx),
|
||||
float(intr.ppy),
|
||||
[float(c) for c in list(intr.coeffs)[:5]],
|
||||
)
|
||||
if best is None or cand[0] < best[0]:
|
||||
best = cand
|
||||
|
||||
if best is None:
|
||||
raise RuntimeError(f'{name} ({sn}) 上没有彩色流 profile')
|
||||
|
||||
_, w, h, fx, fy, cx, cy, coeffs = best
|
||||
while len(coeffs) < 5:
|
||||
coeffs.append(0.0)
|
||||
return RealsenseIntrinsics(
|
||||
model=want,
|
||||
serial=sn,
|
||||
width=w,
|
||||
height=h,
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
cx=cx,
|
||||
cy=cy,
|
||||
dist=coeffs[:5],
|
||||
)
|
||||
|
||||
|
||||
def is_realsense_source(name: str) -> bool:
|
||||
"""对 d405 / d435 及带命名空间的别名（d405_left、d405_right）返回 True。"""
|
||||
return _norm_model(name or "") in ("d405", "d435")
|
||||
|
||||
|
||||
def resolve_realsense_topics(
|
||||
model: str,
|
||||
*,
|
||||
camera_namespace: str = '',
|
||||
image_topic: str = '',
|
||||
camera_info_topic: str = '',
|
||||
) -> Tuple[str, str]:
|
||||
"""为空时，为 d405/d435 填充默认的 image/camera_info 话题。
|
||||
|
||||
``model`` 可以是 ``d405_left`` / ``d405_right`` 这类话题命名空间。
|
||||
"""
|
||||
ns = (camera_namespace or model or 'd405').strip().strip('/')
|
||||
img = (image_topic or '').strip()
|
||||
info = (camera_info_topic or '').strip()
|
||||
if not img and ns:
|
||||
img = f'/{ns}/color/image_raw'
|
||||
if not info and ns:
|
||||
info = f'/{ns}/color/camera_info'
|
||||
return img, info
|
||||
|
||||
|
||||
def apply_realsense_exposure(
|
||||
device,
|
||||
*,
|
||||
auto_exposure: bool = True,
|
||||
ae_limit_us: float = 3000.0,
|
||||
emitter_enabled: bool = False,
|
||||
laser_power: float = 0.0,
|
||||
exposure_us: float | None = None,
|
||||
gain: float | None = None,
|
||||
log=None,
|
||||
) -> list[str]:
|
||||
"""限制自动曝光 / 关闭投影器。腕部 D405 过曝通常是自动曝光过长 + 发射器导致。"""
|
||||
import pyrealsense2 as rs
|
||||
|
||||
notes: list[str] = []
|
||||
|
||||
def _set(sensor, opt, val) -> bool:
|
||||
try:
|
||||
if sensor.supports(opt):
|
||||
sensor.set_option(opt, float(val))
|
||||
return True
|
||||
except Exception:
|
||||
return False
|
||||
return False
|
||||
|
||||
for sensor in device.query_sensors():
|
||||
if _set(sensor, rs.option.emitter_enabled, 1.0 if emitter_enabled else 0.0):
|
||||
notes.append(f"emitter={int(bool(emitter_enabled))}")
|
||||
laser_opt = getattr(rs.option, "laser_power", None)
|
||||
if laser_opt is not None and not emitter_enabled:
|
||||
_set(sensor, laser_opt, float(laser_power))
|
||||
# 必须在写曝光*之前*关闭自动曝光；D405 固件在自动曝光开启时会忽略曝光设置。
|
||||
if _set(sensor, rs.option.enable_auto_exposure, 1.0 if auto_exposure else 0.0):
|
||||
notes.append(f"ae={int(bool(auto_exposure))}")
|
||||
pri = getattr(rs.option, "auto_exposure_priority", None)
|
||||
# priority 0 = 保帧压暗；无上限时不要设，让自动曝光自己变亮
|
||||
if pri is not None and ae_limit_us and auto_exposure:
|
||||
_set(sensor, pri, 0.0)
|
||||
limit = getattr(rs.option, "auto_exposure_limit", None)
|
||||
if auto_exposure and limit is not None and ae_limit_us and ae_limit_us > 0:
|
||||
if _set(sensor, limit, float(ae_limit_us)):
|
||||
notes.append(f"ae_limit={int(ae_limit_us)}us")
|
||||
if not auto_exposure:
|
||||
if exposure_us is not None and _set(sensor, rs.option.exposure, float(exposure_us)):
|
||||
notes.append(f"exposure={int(exposure_us)}us")
|
||||
if gain is not None and _set(sensor, rs.option.gain, float(gain)):
|
||||
notes.append(f"gain={gain}")
|
||||
if log and notes:
|
||||
log("RealSense 曝光设置：" + ", ".join(dict.fromkeys(notes)))
|
||||
return notes
|
||||
@@ -0,0 +1,844 @@
|
||||
"""工件二阶段检测：YOLO 定位工件 + HoughCircles 检测左右螺纹孔中心。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
_PKG_CONFIG = Path(__file__).resolve().parents[1] / "config" / "hole_detect.yaml"
|
||||
_WS_CONFIG = Path.home() / "ros2" / "src" / "arm_application" / "config" / "hole_detect.yaml"
|
||||
DEFAULT_CONFIG = _PKG_CONFIG if _PKG_CONFIG.is_file() else _WS_CONFIG
|
||||
|
||||
|
||||
def load_config(path: str | Path | None = None) -> dict[str, Any]:
|
||||
cfg_path = Path(path) if path else DEFAULT_CONFIG
|
||||
with open(cfg_path, encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f) or {}
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError(f"配置格式错误: {cfg_path}")
|
||||
return data
|
||||
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def _fail(
|
||||
msg: str,
|
||||
draw_img: np.ndarray,
|
||||
bbox: tuple[int, int, int, int] | None = None,
|
||||
gate_block: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
return {
|
||||
"status": False,
|
||||
"msg": msg,
|
||||
"hole_points": [],
|
||||
"hole_radii": [],
|
||||
"bbox": list(bbox) if bbox is not None else None,
|
||||
"gate_block": gate_block,
|
||||
"draw_img": draw_img,
|
||||
}
|
||||
|
||||
|
||||
def _draw_bbox(
|
||||
img: np.ndarray,
|
||||
bbox: tuple[int, int, int, int],
|
||||
color=(0, 255, 0),
|
||||
label: str = "",
|
||||
) -> None:
|
||||
"""画工件框。``label`` 是模型给出的类别名（由标签文件决定）。
|
||||
|
||||
这里刻意**不写死类名**：以前写死了 "gongjian"，但实际模型的类别是
|
||||
"workpiece"，图上的字和模型的类名对不上，排查时会被误导。
|
||||
现在用 ``Detection.cls_name``，也就是 ``weights.sh labels`` 里能读到的那份。
|
||||
"""
|
||||
x1, y1, x2, y2 = bbox
|
||||
cv2.rectangle(img, (x1, y1), (x2, y2), color, 2)
|
||||
text = (label or "").strip()
|
||||
if text:
|
||||
cv2.putText(img, text, (x1, max(20, y1 - 8)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2)
|
||||
|
||||
|
||||
def _best_label(dets) -> str:
|
||||
"""取置信度最高那个检测的类名（由标签文件决定）。没有则返回空串。"""
|
||||
if not dets:
|
||||
return ""
|
||||
try:
|
||||
best = max(dets, key=lambda d: float(d.conf))
|
||||
except Exception: # noqa: BLE001
|
||||
return ""
|
||||
return str(getattr(best, "cls_name", "") or "").strip()
|
||||
|
||||
|
||||
def _draw_center_gate(img: np.ndarray, max_dx: float, max_dy: float, ok: bool) -> None:
|
||||
h, w = img.shape[:2]
|
||||
cx, cy = w * 0.5, h * 0.5
|
||||
x1 = int(round(cx - max_dx))
|
||||
y1 = int(round(cy - max_dy))
|
||||
x2 = int(round(cx + max_dx))
|
||||
y2 = int(round(cy + max_dy))
|
||||
color = (0, 255, 0) if ok else (0, 165, 255)
|
||||
cv2.rectangle(img, (x1, y1), (x2, y2), color, 1)
|
||||
cv2.drawMarker(img, (int(cx), int(cy)), color, markerType=cv2.MARKER_CROSS, markerSize=18, thickness=1)
|
||||
|
||||
|
||||
def _bbox_center_ok(
|
||||
bbox: tuple[int, int, int, int],
|
||||
img_w: int,
|
||||
img_h: int,
|
||||
cfg: dict[str, Any],
|
||||
) -> tuple[bool, float, float, float, float]:
|
||||
"""返回 (是否居中, bbox_cx, bbox_cy, max_dx, max_dy)。"""
|
||||
x1, y1, x2, y2 = bbox
|
||||
bx = 0.5 * (x1 + x2)
|
||||
by = 0.5 * (y1 + y2)
|
||||
max_dx = float(cfg.get("center_max_offset_ratio_x", 0.12)) * img_w
|
||||
max_dy = float(cfg.get("center_max_offset_ratio_y", 0.12)) * img_h
|
||||
ok = abs(bx - img_w * 0.5) <= max_dx and abs(by - img_h * 0.5) <= max_dy
|
||||
return ok, bx, by, max_dx, max_dy
|
||||
|
||||
|
||||
def _draw_holes(
|
||||
img: np.ndarray,
|
||||
points: list[list[float]],
|
||||
radii: list[float] | None = None,
|
||||
) -> None:
|
||||
"""画孔轮廓圆 + 中心点（十字+实心点）+ 坐标。"""
|
||||
for i, (cx, cy) in enumerate(points):
|
||||
px, py = int(round(cx)), int(round(cy))
|
||||
r = 12.0 if radii is None or i >= len(radii) else float(radii[i])
|
||||
rr = max(4, int(round(r)))
|
||||
# 轮廓
|
||||
cv2.circle(img, (px, py), rr, (0, 0, 255), 2)
|
||||
# 中心十字 + 黄点
|
||||
cv2.drawMarker(img, (px, py), (0, 0, 255), markerType=cv2.MARKER_CROSS, markerSize=16, thickness=2)
|
||||
cv2.circle(img, (px, py), 3, (0, 255, 255), -1)
|
||||
label = f"P{i + 1}({px},{py}) r={rr}"
|
||||
cv2.putText(img, label, (px + 12, py - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 0, 255), 2)
|
||||
|
||||
|
||||
def _parse_best_bbox(dets, conf_thr: float) -> tuple[int, int, int, int] | None:
|
||||
"""最多取 1 个工件：取置信度最高且 >= conf 的框。"""
|
||||
if not dets:
|
||||
return None
|
||||
ordered = sorted(dets, key=lambda d: d.conf, reverse=True)
|
||||
for det in ordered:
|
||||
if float(det.conf) < conf_thr:
|
||||
continue
|
||||
return tuple(int(v) for v in det.xyxy) # type: ignore[return-value]
|
||||
return None
|
||||
|
||||
|
||||
def _keypoint_holes(
|
||||
dets,
|
||||
*,
|
||||
conf_thr: float,
|
||||
min_kconf: float,
|
||||
radius_px: float,
|
||||
min_dist_px: float,
|
||||
):
|
||||
"""pose 模型直接给出的 k1/k2 关键点当孔位。
|
||||
|
||||
现场约定（见 ``pose_detector``）：k0 跟踪、k1/k2 对孔。
|
||||
返回 ``((k1x,k1y),(k2x,k2y),r,r)``，拿不到可信关键点时返回 None
|
||||
（不是 pose 模型 / 关键点置信度不够 / 两点几乎重合），调用方回落到找圆。
|
||||
|
||||
为什么值得优先用关键点：Hough / 亮环那套是在**孔本身的图像特征**上找圆，
|
||||
光照、反光、通止规尖伸进视野都会把圆心拉偏；而关键点是模型在训练分布里
|
||||
直接回归出来的，对这几类干扰天然更稳。
|
||||
"""
|
||||
if not dets:
|
||||
return None
|
||||
for det in sorted(dets, key=lambda d: d.conf, reverse=True):
|
||||
if float(det.conf) < conf_thr:
|
||||
continue
|
||||
kpts = getattr(det, "kpts", None)
|
||||
if kpts is None:
|
||||
continue
|
||||
kpts = np.asarray(kpts, dtype=float)
|
||||
if kpts.ndim != 2 or kpts.shape[0] < 3 or kpts.shape[1] < 3:
|
||||
continue
|
||||
k1, k2 = kpts[1].copy(), kpts[2].copy()
|
||||
if not (np.isfinite(k1[:2]).all() and np.isfinite(k2[:2]).all()):
|
||||
continue
|
||||
if float(k1[2]) < min_kconf or float(k2[2]) < min_kconf:
|
||||
logger.info(
|
||||
"k1/k2 关键点置信度不足（%.2f / %.2f < %.2f）—— 回落到找圆",
|
||||
float(k1[2]),
|
||||
float(k2[2]),
|
||||
min_kconf,
|
||||
)
|
||||
continue
|
||||
dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
|
||||
if dist < float(min_dist_px):
|
||||
logger.info(
|
||||
"k1/k2 两关键点只差 %.1fpx（< %.1fpx）—— 视为不可信，回落到找圆",
|
||||
dist,
|
||||
min_dist_px,
|
||||
)
|
||||
continue
|
||||
r = float(radius_px)
|
||||
# **刻意不按图像 x 交换左右**。
|
||||
# Hough 路径的 k1/k2 是"画面左孔 / 右孔"（位置定义），但 pose 模型学出来的
|
||||
# k1/k2 是**固定语义**（训练时标注的那个物理孔）。工件只要转个角度，
|
||||
# 位置就会翻面；按 x 强制交换会让 `target_hole=k1` 时左时右地指向两个不同
|
||||
# 的物理孔 —— 那是最难查的一类现场问题。
|
||||
# 下游怎么认这两个点由 `stage2.k1_label` 决定：
|
||||
# hole_points[0] = 模型的 k1，hole_points[1] = 模型的 k2，
|
||||
# 所以 k1_label="left" ↔ k1 就是模型的 k1（见 stage2_locate_node._snap）。
|
||||
return (float(k1[0]), float(k1[1])), (float(k2[0]), float(k2[1])), r, r
|
||||
return None
|
||||
|
||||
|
||||
def _refine_circle_center(
|
||||
gray: np.ndarray,
|
||||
cx: float,
|
||||
cy: float,
|
||||
r: float,
|
||||
) -> tuple[float, float]:
|
||||
"""Hough 整数圆心 → 暗孔连通域一阶矩（亚像素）。失败则退回原圆心。"""
|
||||
h, w = gray.shape[:2]
|
||||
rad = max(5.0, float(r))
|
||||
half = int(round(2.2 * rad))
|
||||
x0 = max(0, int(round(cx)) - half)
|
||||
y0 = max(0, int(round(cy)) - half)
|
||||
x1 = min(w, int(round(cx)) + half)
|
||||
y1 = min(h, int(round(cy)) + half)
|
||||
if x1 - x0 < 8 or y1 - y0 < 8:
|
||||
return cx, cy
|
||||
roi = gray[y0:y1, x0:x1]
|
||||
blur = cv2.GaussianBlur(roi, (5, 5), 1.0)
|
||||
_, bw = cv2.threshold(blur, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
|
||||
n, labels, stats, cents = cv2.connectedComponentsWithStats(bw, 8)
|
||||
if n <= 1:
|
||||
return cx, cy
|
||||
ex, ey = cx - x0, cy - y0
|
||||
min_area = np.pi * (0.35 * rad) ** 2
|
||||
max_area = np.pi * (1.8 * rad) ** 2
|
||||
best_i = 0
|
||||
best_d = 1e18
|
||||
for i in range(1, n):
|
||||
area = float(stats[i, cv2.CC_STAT_AREA])
|
||||
if area < min_area or area > max_area:
|
||||
continue
|
||||
d = (float(cents[i][0]) - ex) ** 2 + (float(cents[i][1]) - ey) ** 2
|
||||
if d < best_d:
|
||||
best_d = d
|
||||
best_i = i
|
||||
if best_i <= 0 or best_d > (1.2 * rad) ** 2:
|
||||
return cx, cy
|
||||
mask = np.uint8(labels == best_i)
|
||||
M = cv2.moments(mask)
|
||||
if M["m00"] < 1.0:
|
||||
return cx, cy
|
||||
return x0 + M["m10"] / M["m00"], y0 + M["m01"] / M["m00"]
|
||||
|
||||
|
||||
# ==================== 螺纹嵌件对检测（亮环签名 + 关于中心孔对称）====================
|
||||
# 目标件是圆形法兰：中心一个大螺纹孔，两个目标螺纹嵌件在中心孔两侧、关于中心对称。
|
||||
# 旧 Hough"间距最大的一对"在这类多孔板上会锁到普通安装孔且逐帧跳变。
|
||||
# 新路径：金属嵌件有"暗孔芯 + 亮金属环 + 暗法兰背景"的明确签名；
|
||||
# 两个目标嵌件关于中心孔对称（中点≈中心孔、半径相近）——据此唯一锁定，抗干扰抗跳变。
|
||||
|
||||
|
||||
def _disc_kernel(r: float) -> np.ndarray:
|
||||
R = int(np.ceil(r))
|
||||
y, x = np.mgrid[-R:R + 1, -R:R + 1]
|
||||
k = (x * x + y * y <= r * r).astype(np.float32)
|
||||
return k / max(1.0, float(k.sum()))
|
||||
|
||||
|
||||
def _ring_kernel(a: float, b: float) -> np.ndarray:
|
||||
R = int(np.ceil(b))
|
||||
y, x = np.mgrid[-R:R + 1, -R:R + 1]
|
||||
d = x * x + y * y
|
||||
k = ((d >= a * a) & (d <= b * b)).astype(np.float32)
|
||||
return k / max(1.0, float(k.sum()))
|
||||
|
||||
|
||||
def _insert_score_map(gray: np.ndarray, scales) -> np.ndarray:
|
||||
"""亮环签名评分（多尺度取 max）：环越亮、芯越暗、外背景越暗，分越高。"""
|
||||
g = gray.astype(np.float32)
|
||||
best = None
|
||||
for ri, rr, ro in scales:
|
||||
inner = cv2.filter2D(g, -1, _disc_kernel(ri))
|
||||
ringm = cv2.filter2D(g, -1, _ring_kernel(ri + 1, rr))
|
||||
outer = cv2.filter2D(g, -1, _ring_kernel(rr + 2, ro))
|
||||
s = ringm - 0.5 * (inner + outer)
|
||||
best = s if best is None else np.maximum(best, s)
|
||||
return best
|
||||
|
||||
|
||||
def _refine_dark_bore(gray: np.ndarray, cx: float, cy: float, half: int = 26):
|
||||
"""暗孔连通域一阶矩 → 亚像素孔心 + 等效半径。失败退回原心。"""
|
||||
h, w = gray.shape
|
||||
x0 = max(0, int(round(cx)) - half)
|
||||
y0 = max(0, int(round(cy)) - half)
|
||||
x1 = min(w, int(round(cx)) + half)
|
||||
y1 = min(h, int(round(cy)) + half)
|
||||
if x1 - x0 < 12 or y1 - y0 < 12:
|
||||
return float(cx), float(cy), 12.0
|
||||
roi = gray[y0:y1, x0:x1].astype(np.float32)
|
||||
blur = cv2.GaussianBlur(roi, (5, 5), 1.0)
|
||||
thr = float(np.percentile(blur, 20))
|
||||
mask = (blur < thr).astype(np.uint8) * 255
|
||||
n, _lab, st, ce = cv2.connectedComponentsWithStats(mask, 8)
|
||||
best_i, best_d = 0, 1e18
|
||||
ex, ey = cx - x0, cy - y0
|
||||
for i in range(1, n):
|
||||
a = float(st[i, cv2.CC_STAT_AREA])
|
||||
if a < 15:
|
||||
continue
|
||||
d = (float(ce[i][0]) - ex) ** 2 + (float(ce[i][1]) - ey) ** 2
|
||||
if d < best_d:
|
||||
best_d, best_i = d, i
|
||||
if best_i <= 0:
|
||||
return float(cx), float(cy), 12.0
|
||||
area = float(st[best_i, cv2.CC_STAT_AREA])
|
||||
r_eq = float(np.sqrt(area / np.pi))
|
||||
return x0 + float(ce[best_i][0]), y0 + float(ce[best_i][1]), max(6.0, r_eq)
|
||||
|
||||
|
||||
def _detect_insert_pair(gray: np.ndarray, bbox, cfg, depth_m=None, intr=None):
|
||||
"""找"关于工件中心对称的一对螺纹嵌件"。返回 ((k1x,k1y),(k2x,k2y),r1,r2) 或 None。
|
||||
|
||||
尺度自适应：用深度把物理螺栓圆半径（~29mm，两孔间距~58mm）换算成像素半径，
|
||||
在中心 C 周围的圆环上搜候选，再选"中点≈C、都在环上、间距≈直径"的对称对。
|
||||
不依赖 YOLO 框是否盖住嵌件（框只用来定中心 C）。k1=左孔(x 小)，k2=右孔(x 大)。
|
||||
"""
|
||||
h, w = gray.shape
|
||||
x1, y1, x2, y2 = [int(v) for v in bbox]
|
||||
C = np.array([0.5 * (x1 + x2), 0.5 * (y1 + y2)], dtype=float)
|
||||
# ---- 螺栓圆像素半径 bolt_px ----
|
||||
bolt_mm = float(cfg.get("insert_bolt_radius_mm", 29.0))
|
||||
bolt_px = 0.0
|
||||
if depth_m is not None and intr is not None:
|
||||
# 工件表面深度：C 附近小邻域的浅分位（孔底更深，表面偏浅）
|
||||
r_s = max(6, int(round(0.25 * min(x2 - x1, y2 - y1))))
|
||||
cxi, cyi = int(round(C[0])), int(round(C[1]))
|
||||
sx1, sx2 = max(0, cxi - r_s), min(w, cxi + r_s)
|
||||
sy1, sy2 = max(0, cyi - r_s), min(h, cyi + r_s)
|
||||
zreg = depth_m[sy1:sy2, sx1:sx2]
|
||||
zreg = zreg[np.isfinite(zreg) & (zreg > 0.05)]
|
||||
if zreg.size >= 20:
|
||||
z_plate = float(np.percentile(zreg, 30))
|
||||
z_surf = max(0.05, z_plate - float(cfg.get("plate_thickness_m", 0.004)))
|
||||
bolt_px = bolt_mm / 1000.0 * float(intr["fx"]) / z_surf
|
||||
if bolt_px <= 0:
|
||||
# 无深度时退化为按框尺寸粗估（嵌件约在 0.7×框半宽处）
|
||||
bolt_px = 0.70 * 0.5 * max(x2 - x1, y2 - y1)
|
||||
bolt_px = float(np.clip(bolt_px, 24.0, 0.42 * min(h, w)))
|
||||
# ---- 亮环分数图 ----
|
||||
scales = cfg.get("insert_scales") or [
|
||||
(3, 6, 10), (5, 9, 14), (7, 12, 18), (10, 16, 23), (14, 21, 30),
|
||||
]
|
||||
score = _insert_score_map(gray, scales)
|
||||
# ---- 圆环掩码：嵌件在 [0.45, 1.55]×bolt_px 的环上 ----
|
||||
yy, xx = np.mgrid[0:h, 0:w]
|
||||
dist = np.hypot(xx - C[0], yy - C[1])
|
||||
ring = ((dist > 0.45 * bolt_px) & (dist < 1.55 * bolt_px)).astype(np.float32)
|
||||
scm = score * ring
|
||||
pos = scm[scm > 0]
|
||||
if pos.size < 20:
|
||||
return None
|
||||
thr = float(np.percentile(pos, float(cfg.get("insert_percentile", 96))))
|
||||
pk = (scm > thr).astype(np.uint8) * 255
|
||||
n, _lab, st, ce = cv2.connectedComponentsWithStats(pk, 8)
|
||||
cands = [] # (pos, score)
|
||||
for i in range(1, n):
|
||||
if st[i, cv2.CC_STAT_AREA] < 8:
|
||||
continue
|
||||
cx_i, cy_i = float(ce[i][0]), float(ce[i][1])
|
||||
cands.append((np.array([cx_i, cy_i], dtype=float),
|
||||
float(scm[int(round(cy_i)), int(round(cx_i))])))
|
||||
if len(cands) < 2:
|
||||
return None
|
||||
# ---- 对称对：两孔都在环上、间距≈直径、中点大致≈C（对称门）。
|
||||
# 在几何合格的对里选"亮环总分最高"的——真嵌件亮环最强，稳定选中真孔对，
|
||||
# 不被"更对称但更弱"的假对带偏（C 与真中心有几像素偏差时仍稳健，消除双稳态跳变）。
|
||||
best = None
|
||||
best_score = -1.0
|
||||
sym_gate = float(cfg.get("insert_sym_gate_ratio", 0.40)) * bolt_px
|
||||
for i in range(len(cands)):
|
||||
for j in range(i + 1, len(cands)):
|
||||
pi, si = cands[i]
|
||||
pj, sj = cands[j]
|
||||
di = float(np.linalg.norm(pi - C))
|
||||
dj = float(np.linalg.norm(pj - C))
|
||||
if not (0.40 * bolt_px < di < 1.65 * bolt_px):
|
||||
continue
|
||||
if not (0.40 * bolt_px < dj < 1.65 * bolt_px):
|
||||
continue
|
||||
sep = float(np.linalg.norm(pi - pj))
|
||||
if not (1.2 * bolt_px < sep < 2.9 * bolt_px):
|
||||
continue
|
||||
mid = 0.5 * (pi + pj)
|
||||
if float(np.linalg.norm(mid - C)) > sym_gate:
|
||||
continue
|
||||
tot = si + sj
|
||||
if tot > best_score:
|
||||
best_score = tot
|
||||
best = (pi, pj)
|
||||
if best is None:
|
||||
return None
|
||||
p, q = best
|
||||
# 稳定分配 k1/k2：沿主方向，避免竖直对时"x 小者"被噪声翻转。
|
||||
# 竖直对(|dy|>=|dx|) → k1=上(y 小)、k2=下；水平对 → k1=左(x 小)、k2=右。
|
||||
if abs(p[1] - q[1]) >= abs(p[0] - q[0]):
|
||||
k1raw, k2raw = (p, q) if p[1] < q[1] else (q, p)
|
||||
else:
|
||||
k1raw, k2raw = (p, q) if p[0] < q[0] else (q, p)
|
||||
k1x, k1y, r1 = _refine_dark_bore(gray, k1raw[0], k1raw[1])
|
||||
k2x, k2y, r2 = _refine_dark_bore(gray, k2raw[0], k2raw[1])
|
||||
return (k1x, k1y), (k2x, k2y), r1, r2
|
||||
|
||||
|
||||
def _pad_roi(
|
||||
img_w: int,
|
||||
img_h: int,
|
||||
bbox: tuple[int, int, int, int],
|
||||
padding: int,
|
||||
) -> tuple[int, int, int, int, int, int]:
|
||||
"""返回 x1,y1,x2,y2,offset_x,offset_y（裁剪左上角在原图坐标）。"""
|
||||
x1, y1, x2, y2 = bbox
|
||||
px1 = max(0, x1 - padding)
|
||||
py1 = max(0, y1 - padding)
|
||||
px2 = min(img_w, x2 + padding)
|
||||
py2 = min(img_h, y2 + padding)
|
||||
if px2 <= px1 or py2 <= py1:
|
||||
raise ValueError("ROI 无效（宽或高为 0）")
|
||||
return px1, py1, px2, py2, px1, py1
|
||||
|
||||
|
||||
def _render_success(
|
||||
image: np.ndarray,
|
||||
bbox: tuple[int, int, int, int],
|
||||
hole_points: list[list[float]],
|
||||
hole_radii: list[float],
|
||||
dist: float,
|
||||
gate: tuple[float, float] | None = None,
|
||||
label: str = "",
|
||||
) -> np.ndarray:
|
||||
draw_img = image.copy()
|
||||
if gate is not None:
|
||||
_draw_center_gate(draw_img, gate[0], gate[1], True)
|
||||
_draw_bbox(draw_img, bbox, label=label)
|
||||
_draw_holes(draw_img, hole_points, hole_radii)
|
||||
cv2.putText(
|
||||
draw_img,
|
||||
f"OK dist={dist:.1f}px",
|
||||
(16, 36),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.8,
|
||||
(0, 255, 0),
|
||||
2,
|
||||
)
|
||||
return draw_img
|
||||
|
||||
|
||||
def detect_holes(
|
||||
image: np.ndarray,
|
||||
detector,
|
||||
config: dict[str, Any] | None = None,
|
||||
device: str | int = 0,
|
||||
skip_center_gate: bool = False,
|
||||
depth_m: np.ndarray | None = None,
|
||||
intr: dict | None = None,
|
||||
allow_full_frame_roi: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""
|
||||
二阶段检测螺纹孔中心。
|
||||
|
||||
detector: 需提供 detect(image_bgr, conf=..., iou=..., imgsz=...) -> list[Detection]
|
||||
（YOLODetector / FasterRCNNDetector / ONNX 均可）
|
||||
|
||||
:param allow_full_frame_roi: 检测器**没给出工件框**时，是否退回"整幅画面当 ROI"。
|
||||
默认 False（保持原行为：没有框就不做，报「未检测到工件」）。
|
||||
置 True 的场景是**调用方明确要试一把**：知道检测器可能漏检，但孔本身
|
||||
在画面里已经看得见（例如工件检测模型与当前相机/画面不匹配、或工件贴着
|
||||
画面中心）。此时 ROI 的约束由孔自身的判据接手 —— 环半径、两孔间距、
|
||||
居中门控都还在，所以不会凭空多出"随便一个圆"。
|
||||
|
||||
返回:
|
||||
status, msg, hole_points [[cx,cy],...], hole_radii [r1,r2], bbox, draw_img(BGR)
|
||||
"""
|
||||
try:
|
||||
if image is None or not isinstance(image, np.ndarray) or image.size == 0:
|
||||
return _fail("输入图像无效", np.zeros((480, 640, 3), dtype=np.uint8))
|
||||
|
||||
cfg = config if config is not None else load_config()
|
||||
draw_img = image.copy()
|
||||
h, w = image.shape[:2]
|
||||
|
||||
conf = float(cfg.get("conf", 0.25))
|
||||
iou = float(cfg.get("iou", 0.7))
|
||||
imgsz = int(cfg.get("imgsz", 640))
|
||||
padding = int(cfg.get("bbox_padding", 25))
|
||||
ksize = int(cfg.get("gaussian_ksize", 5))
|
||||
if ksize % 2 == 0:
|
||||
ksize += 1
|
||||
sigma = float(cfg.get("gaussian_sigma", 1.5))
|
||||
|
||||
# 1) 检测器定位工件 bbox（最多 1 个）— 兼容 YOLO / Faster R-CNN
|
||||
dets = detector.detect(image, conf=conf, iou=iou, imgsz=imgsz)
|
||||
bbox = _parse_best_bbox(dets, conf)
|
||||
if bbox is None and allow_full_frame_roi:
|
||||
# 退回整幅画面：ROI 从"检测器给的框"换成"整幅"，孔自身的判据
|
||||
#（环半径区间、两孔间距、居中门控）继续把关。
|
||||
bbox = (0, 0, int(w - 1), int(h - 1))
|
||||
logger.info(
|
||||
"工件检测器没给框，按调用方要求用整幅画面当 ROI 继续找孔"
|
||||
"（允许的原因见 allow_full_frame_roi 的说明）"
|
||||
)
|
||||
if bbox is None:
|
||||
return _fail("未检测到工件", draw_img)
|
||||
|
||||
# 类别名来自标签文件（见 model_labels / weights.sh labels），不再写死。
|
||||
label = _best_label(dets)
|
||||
_draw_bbox(draw_img, bbox, label=label)
|
||||
|
||||
# 1.5) 居中门控：bbox 中心接近画面中心才进入二阶检测。
|
||||
# 精对准 k1/k2 时工件会离开画面中心，调用方应 skip_center_gate=True。
|
||||
center_enable = bool(cfg.get("center_gate_enable", True)) and not skip_center_gate
|
||||
centered, bx, by, max_dx, max_dy = _bbox_center_ok(bbox, w, h, cfg)
|
||||
if bool(cfg.get("center_gate_draw", True)):
|
||||
_draw_center_gate(draw_img, max_dx, max_dy, centered if center_enable else True)
|
||||
if center_enable and not centered:
|
||||
dx = abs(bx - w * 0.5)
|
||||
dy = abs(by - h * 0.5)
|
||||
cv2.putText(
|
||||
draw_img,
|
||||
"MOVE TO CENTER",
|
||||
(16, 36),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.8,
|
||||
(0, 165, 255),
|
||||
2,
|
||||
)
|
||||
return _fail(
|
||||
f"工件未居中：offset=({dx:.0f},{dy:.0f})px，允许<=({max_dx:.0f},{max_dy:.0f})px",
|
||||
draw_img,
|
||||
bbox,
|
||||
gate_block=True,
|
||||
)
|
||||
|
||||
# ---- 首选：pose 模型直接给出 k1/k2 关键点（不必再找圆） ----
|
||||
if bool(cfg.get("keypoint_enable", True)):
|
||||
kp = _keypoint_holes(
|
||||
dets,
|
||||
conf_thr=conf,
|
||||
min_kconf=float(cfg.get("keypoint_min_conf", 0.30)),
|
||||
radius_px=float(cfg.get("keypoint_hole_radius_px", 12.0)),
|
||||
min_dist_px=float(cfg.get("keypoint_min_dist_px", 15.0)),
|
||||
)
|
||||
if kp is not None:
|
||||
(k1, k2, r1, r2) = kp
|
||||
hole_points = [[float(k1[0]), float(k1[1])], [float(k2[0]), float(k2[1])]]
|
||||
hole_radii = [float(r1), float(r2)]
|
||||
dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
|
||||
gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
|
||||
draw_img = _render_success(
|
||||
image, bbox, hole_points, hole_radii, dist, gate=gate, label=label
|
||||
)
|
||||
return {
|
||||
"status": True,
|
||||
"msg": f"pose 关键点 k1/k2，两孔距离 {dist:.1f}px",
|
||||
"hole_points": hole_points,
|
||||
"hole_radii": hole_radii,
|
||||
"bbox": list(bbox),
|
||||
"label": label,
|
||||
"source": "keypoint",
|
||||
"draw_img": draw_img,
|
||||
}
|
||||
|
||||
# ---- 主路径：螺纹嵌件对（亮环签名 + 关于中心孔对称），抗多孔干扰、抗跳变 ----
|
||||
if bool(cfg.get("insert_pair_enable", True)):
|
||||
gray_full = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
|
||||
pair = _detect_insert_pair(gray_full, bbox, cfg, depth_m=depth_m, intr=intr)
|
||||
if pair is not None:
|
||||
(k1, k2, r1, r2) = pair
|
||||
hole_points = [[float(k1[0]), float(k1[1])], [float(k2[0]), float(k2[1])]]
|
||||
hole_radii = [float(r1), float(r2)]
|
||||
dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
|
||||
gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
|
||||
draw_img = _render_success(
|
||||
image, bbox, hole_points, hole_radii, dist, gate=gate, label=label
|
||||
)
|
||||
return {
|
||||
"status": True,
|
||||
"msg": f"螺纹嵌件对，两孔距离 {dist:.1f}px",
|
||||
"hole_points": hole_points,
|
||||
"hole_radii": hole_radii,
|
||||
"bbox": list(bbox),
|
||||
"label": label,
|
||||
"draw_img": draw_img,
|
||||
}
|
||||
# ---- 退路：原 Hough 老路径 ----
|
||||
|
||||
# YOLO 框经常只包住半边法兰；扩到能同时容纳两个螺纹孔
|
||||
min_w = int(cfg.get("bbox_min_width", 280))
|
||||
min_h = int(cfg.get("bbox_min_height", 220))
|
||||
x1, y1, x2, y2 = bbox
|
||||
bw, bh = x2 - x1, y2 - y1
|
||||
if bw < min_w or bh < min_h:
|
||||
cxb, cyb = 0.5 * (x1 + x2), 0.5 * (y1 + y2)
|
||||
bw, bh = max(bw, min_w), max(bh, min_h)
|
||||
x1 = int(round(cxb - 0.5 * bw))
|
||||
y1 = int(round(cyb - 0.5 * bh))
|
||||
x2 = int(round(cxb + 0.5 * bw))
|
||||
y2 = int(round(cyb + 0.5 * bh))
|
||||
bbox = (
|
||||
max(0, x1), max(0, y1),
|
||||
min(w, x2), min(h, y2),
|
||||
)
|
||||
|
||||
# 2) padding ROI + 边界保护
|
||||
rx1, ry1, rx2, ry2, offset_x, offset_y = _pad_roi(w, h, bbox, padding)
|
||||
roi = image[ry1:ry2, rx1:rx2]
|
||||
if roi.size == 0:
|
||||
return _fail("ROI 裁剪失败", draw_img, bbox)
|
||||
|
||||
# 3) 灰度 + 高斯模糊
|
||||
gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY) if roi.ndim == 3 else roi
|
||||
blur = cv2.GaussianBlur(gray, (ksize, ksize), sigma)
|
||||
|
||||
# 4) HoughCircles
|
||||
circles = cv2.HoughCircles(
|
||||
blur,
|
||||
cv2.HOUGH_GRADIENT,
|
||||
dp=float(cfg.get("hough_dp", 1.2)),
|
||||
minDist=float(cfg.get("hough_min_dist", 40)),
|
||||
param1=float(cfg.get("hough_param1", 100)),
|
||||
param2=float(cfg.get("hough_param2", 28)),
|
||||
minRadius=int(cfg.get("hough_min_radius", 6)),
|
||||
maxRadius=int(cfg.get("hough_max_radius", 45)),
|
||||
)
|
||||
if circles is None:
|
||||
return _fail("HoughCircles 未检测到圆孔", draw_img, bbox)
|
||||
|
||||
# 5) ROI 坐标 -> 原图坐标，再亚像素细化孔心
|
||||
circles = np.asarray(circles[0], dtype=np.float64)
|
||||
gray_full = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
|
||||
do_refine = bool(cfg.get("refine_hole_centers", True))
|
||||
mapped: list[tuple[float, float, float]] = []
|
||||
for cx, cy, r in circles:
|
||||
fx = float(cx + offset_x)
|
||||
fy = float(cy + offset_y)
|
||||
fr = float(r)
|
||||
if do_refine:
|
||||
fx, fy = _refine_circle_center(gray_full, fx, fy, fr)
|
||||
mapped.append((fx, fy, fr))
|
||||
|
||||
# 6) 工业校验（含 bbox padding 区内的孔，避免螺纹孔落在框外）
|
||||
bx1, by1, bx2, by2 = bbox
|
||||
vx1 = max(0, bx1 - padding)
|
||||
vy1 = max(0, by1 - padding)
|
||||
vx2 = min(w, bx2 + padding)
|
||||
vy2 = min(h, by2 + padding)
|
||||
min_r = float(cfg.get("min_radius", 6))
|
||||
max_r = float(cfg.get("max_radius", 45))
|
||||
dist_min = float(cfg.get("hole_distance_min", 80))
|
||||
dist_max = float(cfg.get("hole_distance_max", 900))
|
||||
|
||||
valid: list[tuple[float, float, float]] = []
|
||||
for cx, cy, r in mapped:
|
||||
if not (vx1 <= cx <= vx2 and vy1 <= cy <= vy2):
|
||||
continue
|
||||
if not (min_r <= r <= max_r):
|
||||
continue
|
||||
valid.append((cx, cy, r))
|
||||
|
||||
# 丢掉法兰中心大孔：螺纹孔在工件两端，中心孔靠近 bbox 中心
|
||||
bcx = 0.5 * (bx1 + bx2)
|
||||
bcy = 0.5 * (by1 + by2)
|
||||
diag = max(40.0, float(np.hypot(bx2 - bx1, by2 - by1)))
|
||||
ring = [
|
||||
c for c in valid
|
||||
if float(np.hypot(c[0] - bcx, c[1] - bcy)) >= 0.18 * diag
|
||||
]
|
||||
if len(ring) >= 2:
|
||||
valid = ring
|
||||
|
||||
# 多于 2 个：在间距窗内取半径接近、间距最大的一对（左右螺纹孔，避开中心大孔边缘）
|
||||
if len(valid) >= 2:
|
||||
best = None
|
||||
best_d = -1.0
|
||||
for i in range(len(valid)):
|
||||
for j in range(i + 1, len(valid)):
|
||||
d = float(np.hypot(valid[i][0] - valid[j][0], valid[i][1] - valid[j][1]))
|
||||
if not (dist_min <= d <= dist_max):
|
||||
continue
|
||||
if abs(float(valid[i][2]) - float(valid[j][2])) > 10.0:
|
||||
continue
|
||||
if d > best_d:
|
||||
best_d = d
|
||||
best = (valid[i], valid[j])
|
||||
if best is None:
|
||||
return _fail(
|
||||
f"候选圆孔={len(valid)}，没有间距在 [{dist_min}, {dist_max}] 且半径接近的一对",
|
||||
draw_img,
|
||||
bbox,
|
||||
)
|
||||
valid = [best[0], best[1]]
|
||||
|
||||
if len(valid) != 2:
|
||||
return _fail(f"有效圆孔数量={len(valid)}，要求严格等于 2", draw_img, bbox)
|
||||
|
||||
ordered = sorted(valid, key=lambda c: c[0])
|
||||
(x1, y1, r1), (x2, y2, r2) = ordered
|
||||
dist = float(np.hypot(x1 - x2, y1 - y2))
|
||||
if not (dist_min <= dist <= dist_max):
|
||||
return _fail(
|
||||
f"两孔距离={dist:.1f}px，不在 [{dist_min}, {dist_max}]",
|
||||
draw_img,
|
||||
bbox,
|
||||
)
|
||||
|
||||
hole_points = [[x1, y1], [x2, y2]]
|
||||
hole_radii = [r1, r2]
|
||||
gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
|
||||
draw_img = _render_success(image, bbox, hole_points, hole_radii, dist, gate=gate, label=label)
|
||||
return {
|
||||
"status": True,
|
||||
"msg": f"检测成功，两孔距离 {dist:.1f}px",
|
||||
"hole_points": hole_points,
|
||||
"hole_radii": hole_radii,
|
||||
"bbox": list(bbox),
|
||||
"label": label,
|
||||
"draw_img": draw_img,
|
||||
}
|
||||
except Exception as exc:
|
||||
fallback = image.copy() if isinstance(image, np.ndarray) and image.size else np.zeros((480, 640, 3), dtype=np.uint8)
|
||||
return _fail(f"异常: {exc}", fallback)
|
||||
|
||||
|
||||
class HoleStabilizer:
|
||||
"""时序稳帧：EMA 平滑中心/半径，短暂失败时沿用上一帧结果，抑制闪烁。"""
|
||||
|
||||
def __init__(self, config: dict[str, Any] | None = None) -> None:
|
||||
cfg = config or {}
|
||||
self.cfg = cfg
|
||||
self.hold_frames = int(cfg.get("stabilize_hold_frames", 15))
|
||||
self.ema_alpha = float(cfg.get("stabilize_ema_alpha", 0.35))
|
||||
self.max_jump = float(cfg.get("stabilize_max_jump", 60))
|
||||
self.reset()
|
||||
|
||||
def reset(self) -> None:
|
||||
self.points: list[list[float]] | None = None
|
||||
self.radii: list[float] | None = None
|
||||
self.bbox: tuple[int, int, int, int] | None = None
|
||||
self.miss = 0
|
||||
|
||||
def update(self, image: np.ndarray, result: dict[str, Any]) -> dict[str, Any]:
|
||||
try:
|
||||
# 未居中：不做二阶、不沿用旧孔，立即清空稳帧
|
||||
if result.get("gate_block"):
|
||||
self.reset()
|
||||
out = dict(result)
|
||||
out["hole_points"] = []
|
||||
out["hole_radii"] = []
|
||||
return out
|
||||
|
||||
bbox = result.get("bbox")
|
||||
if bbox is not None and len(bbox) == 4:
|
||||
self.bbox = (int(bbox[0]), int(bbox[1]), int(bbox[2]), int(bbox[3]))
|
||||
|
||||
if result.get("status") and len(result.get("hole_points") or []) == 2:
|
||||
pts = [[float(p[0]), float(p[1])] for p in result["hole_points"]]
|
||||
pts = sorted(pts, key=lambda p: p[0])
|
||||
rad = result.get("hole_radii") or [12.0, 12.0]
|
||||
rad = [float(rad[0]), float(rad[1])] if len(rad) >= 2 else [12.0, 12.0]
|
||||
|
||||
if self.points is not None:
|
||||
jump = max(
|
||||
float(np.hypot(pts[0][0] - self.points[0][0], pts[0][1] - self.points[0][1])),
|
||||
float(np.hypot(pts[1][0] - self.points[1][0], pts[1][1] - self.points[1][1])),
|
||||
)
|
||||
if jump > self.max_jump:
|
||||
# 跳变过大，当作本帧失败，沿用稳定值
|
||||
self.miss += 1
|
||||
else:
|
||||
a = self.ema_alpha
|
||||
self.points = [
|
||||
[
|
||||
(1 - a) * self.points[0][0] + a * pts[0][0],
|
||||
(1 - a) * self.points[0][1] + a * pts[0][1],
|
||||
],
|
||||
[
|
||||
(1 - a) * self.points[1][0] + a * pts[1][0],
|
||||
(1 - a) * self.points[1][1] + a * pts[1][1],
|
||||
],
|
||||
]
|
||||
self.radii = [
|
||||
(1 - a) * self.radii[0] + a * rad[0],
|
||||
(1 - a) * self.radii[1] + a * rad[1],
|
||||
]
|
||||
self.miss = 0
|
||||
else:
|
||||
self.points = pts
|
||||
self.radii = rad
|
||||
self.miss = 0
|
||||
else:
|
||||
self.miss += 1
|
||||
if self.miss > self.hold_frames:
|
||||
self.points = None
|
||||
self.radii = None
|
||||
|
||||
if self.points is None or self.radii is None:
|
||||
# 彻底丢失：只保留当前帧绘制（可能仅有 bbox）
|
||||
out = dict(result)
|
||||
out["hole_points"] = []
|
||||
out["hole_radii"] = []
|
||||
return out
|
||||
|
||||
dist = float(
|
||||
np.hypot(
|
||||
self.points[0][0] - self.points[1][0],
|
||||
self.points[0][1] - self.points[1][1],
|
||||
)
|
||||
)
|
||||
bbox_draw = self.bbox
|
||||
if bbox_draw is None and result.get("bbox") is not None:
|
||||
b = result["bbox"]
|
||||
bbox_draw = (int(b[0]), int(b[1]), int(b[2]), int(b[3]))
|
||||
if bbox_draw is None:
|
||||
# 无框时仍画孔
|
||||
draw_img = image.copy()
|
||||
_draw_holes(draw_img, self.points, self.radii)
|
||||
else:
|
||||
gate = None
|
||||
if bool(self.cfg.get("center_gate_draw", True)):
|
||||
ih, iw = image.shape[:2]
|
||||
max_dx = float(self.cfg.get("center_max_offset_ratio_x", 0.12)) * iw
|
||||
max_dy = float(self.cfg.get("center_max_offset_ratio_y", 0.12)) * ih
|
||||
gate = (max_dx, max_dy)
|
||||
draw_img = _render_success(
|
||||
image,
|
||||
bbox_draw,
|
||||
self.points,
|
||||
self.radii,
|
||||
dist,
|
||||
gate=gate,
|
||||
label=str(result.get("label") or ""),
|
||||
)
|
||||
|
||||
held = self.miss > 0
|
||||
return {
|
||||
"status": True,
|
||||
"msg": (
|
||||
f"检测成功，两孔距离 {dist:.1f}px"
|
||||
+ (f"（稳帧 hold={self.miss}）" if held else "")
|
||||
),
|
||||
"hole_points": [[p[0], p[1]] for p in self.points],
|
||||
"hole_radii": [float(self.radii[0]), float(self.radii[1])],
|
||||
"bbox": list(bbox_draw) if bbox_draw is not None else result.get("bbox"),
|
||||
"draw_img": draw_img,
|
||||
}
|
||||
except Exception as exc:
|
||||
fallback = image.copy() if isinstance(image, np.ndarray) and image.size else np.zeros((480, 640, 3), dtype=np.uint8)
|
||||
return _fail(f"稳帧异常: {exc}", fallback)
|
||||
@@ -0,0 +1,190 @@
|
||||
#!/usr/bin/env python3
|
||||
"""在当前机械臂的 D405 彩色话题上做入孔检测。只检测，不做插入动作。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
from arm_application.handeye_realsense import resolve_realsense_topics
|
||||
from arm_application.hole_detect_hough import HoleStabilizer
|
||||
from arm_application.hole_detector import (
|
||||
HoleDetection,
|
||||
HoleDetectResult,
|
||||
HoleDetector,
|
||||
config_from_site,
|
||||
weights_from_site,
|
||||
)
|
||||
from arm_application.site_io import load_site_dict
|
||||
from arm_msgs.msg import HoleDetection as HoleDetectionMsg
|
||||
from arm_msgs.msg import HoleDetections
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
def _as_bool(val, default: bool = False) -> bool:
|
||||
if val is None:
|
||||
return default
|
||||
if isinstance(val, str):
|
||||
return val.strip().lower() in ("1", "true", "yes", "on")
|
||||
return bool(val)
|
||||
|
||||
|
||||
def _bgr_copy(msg: Image) -> np.ndarray:
|
||||
h, w = int(msg.height), int(msg.width)
|
||||
enc = (msg.encoding or "").lower()
|
||||
buf = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
if enc == "bgr8":
|
||||
return buf.reshape(h, w, 3).copy()
|
||||
if enc == "rgb8":
|
||||
return buf.reshape(h, w, 3)[:, :, ::-1].copy()
|
||||
raise RuntimeError(f"Unsupported encoding {msg.encoding!r}")
|
||||
|
||||
|
||||
def _bgr_to_msg(image: np.ndarray, stamp, frame_id: str) -> Image:
|
||||
msg = Image()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = frame_id
|
||||
msg.height = int(image.shape[0])
|
||||
msg.width = int(image.shape[1])
|
||||
msg.encoding = "bgr8"
|
||||
msg.step = int(image.shape[1] * 3)
|
||||
msg.data = image.tobytes()
|
||||
return msg
|
||||
|
||||
|
||||
class HoleDetectNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("hole_detect")
|
||||
self.declare_parameter("camera_source", os.environ.get("SITE_CAM_NAME") or "d405")
|
||||
self.declare_parameter("image_topic", "")
|
||||
self.declare_parameter("weights", "")
|
||||
self.declare_parameter("config_file", "")
|
||||
self.declare_parameter("backend", "two_stage")
|
||||
self.declare_parameter("device", os.environ.get("SITE_YOLO_DEVICE") or "0")
|
||||
self.declare_parameter("preview", False)
|
||||
|
||||
site = load_site_dict()
|
||||
cam = str(self.get_parameter("camera_source").value or "d405")
|
||||
image_topic, _info = resolve_realsense_topics(cam, image_topic=str(self.get_parameter("image_topic").value or ""))
|
||||
cfg = config_from_site(site)
|
||||
param_backend = str(self.get_parameter("backend").value or "").strip().lower()
|
||||
if param_backend:
|
||||
cfg["backend"] = param_backend
|
||||
backend = str(cfg.get("backend") or "two_stage")
|
||||
weights = str(self.get_parameter("weights").value or "").strip() or weights_from_site(site)
|
||||
self._preview = _as_bool(self.get_parameter("preview").value, False)
|
||||
self._detector = HoleDetector(
|
||||
model_path=weights or None,
|
||||
config=cfg,
|
||||
device=str(self.get_parameter("device").value or "0"),
|
||||
)
|
||||
self._stabilizer = (
|
||||
HoleStabilizer(cfg) if backend in ("two_stage", "hough_roi") else None
|
||||
)
|
||||
self._pub = self.create_publisher(HoleDetections, "/hole_detect/result", 10)
|
||||
self._arr_pub = self.create_publisher(Float32MultiArray, "/hole_detect/holes", 10)
|
||||
self._overlay_pub = self.create_publisher(Image, "/hole_detect/overlay", 10)
|
||||
self.create_subscription(Image, image_topic, self._on_image, qos_profile_sensor_data)
|
||||
self.get_logger().info(
|
||||
f"hole_detect camera={cam} topic={image_topic} backend={backend} "
|
||||
f"weights={self._detector.model_path} site overlay=hole_detect"
|
||||
)
|
||||
|
||||
def _on_image(self, msg: Image) -> None:
|
||||
try:
|
||||
bgr = _bgr_copy(msg)
|
||||
except Exception as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
return
|
||||
result = self._detector.detect(bgr)
|
||||
if self._stabilizer is not None:
|
||||
result = self._stabilize(bgr, result)
|
||||
out = HoleDetections()
|
||||
out.header = msg.header
|
||||
out.ok = bool(result.status)
|
||||
out.message = result.msg
|
||||
for hole in result.holes:
|
||||
hmsg = HoleDetectionMsg()
|
||||
hmsg.u = float(hole.center_px[0])
|
||||
hmsg.v = float(hole.center_px[1])
|
||||
hmsg.radius_px = float(hole.radius_px)
|
||||
hmsg.confidence = float(hole.confidence)
|
||||
hmsg.label = hole.label or ""
|
||||
if hole.bbox:
|
||||
hmsg.bbox = [int(v) for v in hole.bbox]
|
||||
out.holes.append(hmsg)
|
||||
self._pub.publish(out)
|
||||
|
||||
arr = Float32MultiArray()
|
||||
data = [1.0 if result.status else 0.0, float(len(result.holes))]
|
||||
for hole in result.holes:
|
||||
data.extend([hole.center_px[0], hole.center_px[1], hole.radius_px, hole.confidence])
|
||||
arr.data = data
|
||||
self._arr_pub.publish(arr)
|
||||
|
||||
overlay = result.draw_img if result.draw_img is not None else bgr
|
||||
self._overlay_pub.publish(_bgr_to_msg(overlay, msg.header.stamp, msg.header.frame_id))
|
||||
if self._preview:
|
||||
import cv2
|
||||
|
||||
cv2.imshow("hole_detect", overlay)
|
||||
cv2.waitKey(1)
|
||||
|
||||
def _stabilize(self, bgr, result: HoleDetectResult) -> HoleDetectResult:
|
||||
raw = result.raw if isinstance(result.raw, dict) else {}
|
||||
payload = {
|
||||
"status": result.status,
|
||||
"msg": result.msg,
|
||||
"hole_points": raw.get("hole_points")
|
||||
or [list(h.center_px) for h in result.holes],
|
||||
"hole_radii": raw.get("hole_radii") or [h.radius_px for h in result.holes],
|
||||
"bbox": raw.get("bbox") or (list(result.holes[0].bbox) if result.holes and result.holes[0].bbox else None),
|
||||
"draw_img": result.draw_img,
|
||||
"gate_block": bool(raw.get("gate_block")),
|
||||
}
|
||||
stab = self._stabilizer.update(bgr, payload)
|
||||
holes: list[HoleDetection] = []
|
||||
radii = stab.get("hole_radii") or []
|
||||
bbox = stab.get("bbox")
|
||||
bbox_t = tuple(int(v) for v in bbox) if bbox and len(bbox) == 4 else None
|
||||
for i, pt in enumerate(stab.get("hole_points") or []):
|
||||
rr = float(radii[i]) if i < len(radii) else 0.0
|
||||
holes.append(
|
||||
HoleDetection(
|
||||
center_px=(float(pt[0]), float(pt[1])),
|
||||
radius_px=rr,
|
||||
confidence=1.0,
|
||||
bbox=bbox_t,
|
||||
label="left" if i == 0 else "right" if i == 1 else f"h{i + 1}",
|
||||
)
|
||||
)
|
||||
return HoleDetectResult(
|
||||
bool(stab.get("status")),
|
||||
str(stab.get("msg") or ""),
|
||||
holes,
|
||||
stab.get("draw_img", result.draw_img),
|
||||
raw=stab,
|
||||
)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
rclpy.init()
|
||||
node = HoleDetectNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,368 @@
|
||||
"""孔定位模块：加载已训练模型，输出孔中心像素坐标与置信度。
|
||||
|
||||
与工件定位（thread_gauge）完全分离，可独立替换模型与参数。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
_PKG_SHARE_FALLBACK = Path(__file__).resolve().parents[1] / "config"
|
||||
DEFAULT_CONFIG = _PKG_SHARE_FALLBACK / "hole_insert.yaml"
|
||||
|
||||
# 仓库里**实际存在**的孔/工件检测模型，相对仓库根。
|
||||
# 以前这个位置上写的是 ``models/hole_insert/best.pt`` —— 文件根本不存在，属于悬空引用：
|
||||
# 配置齐全时永远走不到这条分支，一旦 site.yaml 缺键就报"模型文件不存在"，
|
||||
# 而打印出来的路径看着特别合理，很容易被误判成"文件被人误删了"。
|
||||
# 改动这个常量时，顺手跑 ``bash scripts/weights.sh doctor`` 确认没有悬空引用。
|
||||
_CANONICAL_HOLE_MODEL = "models/yolo/gongjian0929.onnx"
|
||||
|
||||
|
||||
@dataclass
|
||||
class HoleDetection:
|
||||
"""单孔检测结果。"""
|
||||
|
||||
center_px: tuple[float, float]
|
||||
radius_px: float
|
||||
confidence: float
|
||||
bbox: tuple[int, int, int, int] | None = None
|
||||
cls_id: int = 0
|
||||
label: str = "" # left / right
|
||||
|
||||
|
||||
@dataclass
|
||||
class HoleDetectResult:
|
||||
"""孔定位整体结果。"""
|
||||
|
||||
status: bool
|
||||
msg: str
|
||||
holes: list[HoleDetection]
|
||||
draw_img: np.ndarray
|
||||
raw: dict[str, Any] | None = None
|
||||
|
||||
|
||||
def load_hole_config(path: str | Path | None = None) -> dict[str, Any]:
|
||||
cfg_path = Path(path) if path else DEFAULT_CONFIG
|
||||
with open(cfg_path, encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f) or {}
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError(f"孔定位配置格式错误: {cfg_path}")
|
||||
return data
|
||||
|
||||
|
||||
_HOLE_SITE_SKIP = {"weights", "workpiece_weights", "config"}
|
||||
|
||||
|
||||
def config_from_site(data: dict[str, Any] | None = None) -> dict[str, Any]:
|
||||
"""先取包内 yaml 默认值，再用 ``site.yaml`` 的 ``hole_detect:`` 覆盖。
|
||||
|
||||
以 ``site.yaml`` 为准（operator 的唯一数据源）；``data`` 参数用于测试时直接喂字典。
|
||||
"""
|
||||
from arm_application.site_io import deep_merge, load_site_dict, ros2_root
|
||||
|
||||
site = data if data is not None else load_site_dict()
|
||||
hd = dict(site.get("hole_detect") or {})
|
||||
backend = str(hd.get("backend") or "two_stage").strip().lower()
|
||||
cfg_path = str(hd.get("config") or "").strip()
|
||||
if not cfg_path:
|
||||
root = ros2_root()
|
||||
if backend in ("two_stage", "hough_roi"):
|
||||
cfg_path = str(root / "src" / "arm_application" / "config" / "hole_detect.yaml")
|
||||
else:
|
||||
cfg_path = str(root / "src" / "arm_application" / "config" / "hole_insert.yaml")
|
||||
base = load_hole_config(cfg_path) if Path(cfg_path).is_file() else {}
|
||||
overlay = {k: v for k, v in hd.items() if k not in _HOLE_SITE_SKIP and v != ""}
|
||||
merged = deep_merge(base, overlay)
|
||||
merged["backend"] = backend
|
||||
return merged
|
||||
|
||||
|
||||
def weights_from_site(data: dict[str, Any] | None = None) -> str:
|
||||
from arm_application.site_io import load_site_dict, ros2_root
|
||||
|
||||
site = data if data is not None else load_site_dict()
|
||||
hd = site.get("hole_detect") or {}
|
||||
paths = site.get("paths") or {}
|
||||
backend = str(hd.get("backend") or "two_stage").strip().lower()
|
||||
if backend in ("two_stage", "hough_roi"):
|
||||
return str(
|
||||
hd.get("workpiece_weights")
|
||||
or paths.get("hole_workpiece_weights")
|
||||
or paths.get("yolo_weights")
|
||||
or ""
|
||||
)
|
||||
return str(
|
||||
hd.get("weights")
|
||||
or paths.get("hole_weights")
|
||||
or str(ros2_root() / _CANONICAL_HOLE_MODEL)
|
||||
)
|
||||
|
||||
|
||||
def _default_model_path() -> Path:
|
||||
"""未显式给 ``model_path`` 时的兜底模型路径。
|
||||
|
||||
以前这里枚举了 ``hole_insert/best.pt`` / ``hole_best.pt`` / ``yolo/detect_best.pt``，
|
||||
三个文件在本仓库里**都不存在**，函数却会 ``return candidates[0]`` ——
|
||||
于是永远返回一个不存在的路径（同 ``weights_from_site`` 里那个悬空的 best.pt）。
|
||||
|
||||
现在以 site.yaml 为准（它才是真正的来源），它给的路径存在就用它；
|
||||
否则退到仓库里实际存在的规范模型。这样报错信息里的路径至少是对的。
|
||||
"""
|
||||
configured = weights_from_site()
|
||||
if configured:
|
||||
p = Path(configured).expanduser()
|
||||
if p.is_file():
|
||||
return p
|
||||
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
return ros2_root() / _CANONICAL_HOLE_MODEL
|
||||
|
||||
|
||||
class HoleDetector:
|
||||
"""孔定位检测器：支持 YOLO / HoughCircles 双模式，可配置切换。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
model_path: str | Path | None = None,
|
||||
config: dict[str, Any] | None = None,
|
||||
device: str | int = 0,
|
||||
) -> None:
|
||||
self.cfg = config or load_hole_config()
|
||||
self.device = device
|
||||
self.model_path = Path(model_path) if model_path else _default_model_path()
|
||||
self._detector = None
|
||||
self._backend = self.cfg.get("backend", "yolo").lower()
|
||||
|
||||
def _load_yolo(self):
|
||||
if self._detector is not None:
|
||||
return self._detector
|
||||
from arm_application.yolo_detector import build_detector, resolve_weights
|
||||
|
||||
weights = resolve_weights(
|
||||
self.model_path,
|
||||
backend=str(self.cfg.get("yolo_backend") or "auto"),
|
||||
)
|
||||
# build_detector 会自己识别 pose 模型（k0/k1/k2），不必在这里分支。
|
||||
self._detector = build_detector(
|
||||
weights,
|
||||
conf=float(self.cfg.get("conf", 0.25)),
|
||||
iou=float(self.cfg.get("iou", 0.7)),
|
||||
device=str(self.device),
|
||||
imgsz=int(self.cfg.get("imgsz", 640)),
|
||||
)
|
||||
return self._detector
|
||||
|
||||
def detect(self, image_bgr: np.ndarray) -> HoleDetectResult:
|
||||
"""检测图像中的孔，返回像素坐标列表。"""
|
||||
if image_bgr is None or image_bgr.size == 0:
|
||||
return HoleDetectResult(False, "输入图像无效", [], np.zeros((480, 640, 3), dtype=np.uint8))
|
||||
|
||||
draw_img = image_bgr.copy()
|
||||
backend = self._backend
|
||||
|
||||
if backend in {"yolo", "faster_rcnn"}:
|
||||
return self._detect_by_model(image_bgr, draw_img)
|
||||
if backend in {"two_stage", "hough_roi"}:
|
||||
return self._detect_two_stage(image_bgr, draw_img)
|
||||
if backend == "hough":
|
||||
return self._detect_by_hough(image_bgr, draw_img)
|
||||
return HoleDetectResult(False, f"未知 backend: {backend}", [], draw_img)
|
||||
|
||||
def _detect_two_stage(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
|
||||
from arm_application.hole_detect_hough import detect_holes
|
||||
|
||||
try:
|
||||
detector = self._load_yolo()
|
||||
except Exception as exc:
|
||||
return HoleDetectResult(False, f"模型加载失败: {exc}", [], draw_img)
|
||||
|
||||
raw = detect_holes(image, detector, config=self.cfg, device=self.device)
|
||||
holes: list[HoleDetection] = []
|
||||
points = raw.get("hole_points") or []
|
||||
radii = raw.get("hole_radii") or []
|
||||
bbox = raw.get("bbox")
|
||||
bbox_t = tuple(int(v) for v in bbox) if bbox and len(bbox) == 4 else None
|
||||
for i, pt in enumerate(points):
|
||||
r = float(radii[i]) if i < len(radii) else 0.0
|
||||
label = "left" if i == 0 else "right" if i == 1 else f"h{i + 1}"
|
||||
holes.append(
|
||||
HoleDetection(
|
||||
center_px=(float(pt[0]), float(pt[1])),
|
||||
radius_px=r,
|
||||
confidence=1.0,
|
||||
bbox=bbox_t,
|
||||
label=label,
|
||||
)
|
||||
)
|
||||
draw = raw.get("draw_img", draw_img)
|
||||
return HoleDetectResult(bool(raw.get("status")), str(raw.get("msg") or ""), holes, draw, raw=raw)
|
||||
|
||||
def _detect_by_model(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
|
||||
try:
|
||||
detector = self._load_yolo()
|
||||
except Exception as exc:
|
||||
return HoleDetectResult(False, f"模型加载失败: {exc}", [], draw_img)
|
||||
|
||||
conf = float(self.cfg.get("conf", 0.25))
|
||||
iou = float(self.cfg.get("iou", 0.7))
|
||||
imgsz = int(self.cfg.get("imgsz", 640))
|
||||
|
||||
dets = detector.detect(image, conf=conf, iou=iou, imgsz=imgsz)
|
||||
if not dets:
|
||||
return HoleDetectResult(False, "未检测到孔", [], draw_img)
|
||||
|
||||
holes: list[HoleDetection] = []
|
||||
for d in dets:
|
||||
x1, y1, x2, y2 = [int(v) for v in d.xyxy]
|
||||
cx = 0.5 * (x1 + x2)
|
||||
cy = 0.5 * (y1 + y2)
|
||||
r = 0.5 * max(x2 - x1, y2 - y1)
|
||||
holes.append(
|
||||
HoleDetection(
|
||||
center_px=(float(cx), float(cy)),
|
||||
radius_px=float(r),
|
||||
confidence=float(d.conf),
|
||||
bbox=(x1, y1, x2, y2),
|
||||
cls_id=int(getattr(d, "cls_id", 0)),
|
||||
)
|
||||
)
|
||||
return _finalize_holes(holes, draw_img, raw={"dets": dets}, cfg=self.cfg)
|
||||
|
||||
def _detect_by_hough(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
|
||||
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
|
||||
ksize = int(self.cfg.get("gaussian_ksize", 5))
|
||||
if ksize % 2 == 0:
|
||||
ksize += 1
|
||||
blur = cv2.GaussianBlur(gray, (ksize, ksize), float(self.cfg.get("gaussian_sigma", 1.5)))
|
||||
|
||||
circles = cv2.HoughCircles(
|
||||
blur,
|
||||
cv2.HOUGH_GRADIENT,
|
||||
dp=float(self.cfg.get("hough_dp", 1.2)),
|
||||
minDist=float(self.cfg.get("hough_min_dist", 40)),
|
||||
param1=float(self.cfg.get("hough_param1", 100)),
|
||||
param2=float(self.cfg.get("hough_param2", 28)),
|
||||
minRadius=int(self.cfg.get("hough_min_radius", 6)),
|
||||
maxRadius=int(self.cfg.get("hough_max_radius", 45)),
|
||||
)
|
||||
if circles is None:
|
||||
return HoleDetectResult(False, "HoughCircles 未检测到孔", [], draw_img)
|
||||
|
||||
holes: list[HoleDetection] = []
|
||||
for c in np.asarray(circles[0], dtype=np.float64):
|
||||
cx, cy, r = float(c[0]), float(c[1]), float(c[2])
|
||||
holes.append(HoleDetection(center_px=(cx, cy), radius_px=r, confidence=1.0))
|
||||
return _finalize_holes(holes, draw_img, cfg=self.cfg)
|
||||
|
||||
def pixel_to_camera(
|
||||
self,
|
||||
hole: HoleDetection,
|
||||
depth_m: float | None = None,
|
||||
intrinsics: dict[str, float] | None = None,
|
||||
) -> np.ndarray:
|
||||
depth = float(depth_m if depth_m is not None else self.cfg.get("default_depth_m", 0.10))
|
||||
intr = intrinsics or self.cfg.get("camera_intrinsics") or {}
|
||||
return pixel_to_camera(hole, depth, intr)
|
||||
|
||||
|
||||
def pair_two_holes(holes: list[HoleDetection], cfg: dict[str, Any] | None = None) -> list[HoleDetection]:
|
||||
"""取左右两孔：优先最高置信度，再按图像 x 排序并打 left/right 标签。"""
|
||||
cfg = cfg or {}
|
||||
expected = int(cfg.get("expected_holes", 2))
|
||||
if expected != 2:
|
||||
ordered = sorted(holes, key=lambda h: h.center_px[0])
|
||||
for i, hole in enumerate(ordered):
|
||||
hole.label = f"h{i + 1}"
|
||||
return ordered
|
||||
|
||||
if len(holes) < 2:
|
||||
return holes
|
||||
|
||||
picked = sorted(holes, key=lambda h: h.confidence, reverse=True)
|
||||
if len(picked) > 2:
|
||||
by_x = sorted(picked, key=lambda h: h.center_px[0])
|
||||
picked = [by_x[0], by_x[-1]]
|
||||
else:
|
||||
picked = picked[:2]
|
||||
picked.sort(key=lambda h: h.center_px[0])
|
||||
dist = float(
|
||||
np.hypot(
|
||||
picked[1].center_px[0] - picked[0].center_px[0],
|
||||
picked[1].center_px[1] - picked[0].center_px[1],
|
||||
)
|
||||
)
|
||||
min_sep = float(cfg.get("min_hole_sep_px", 20.0))
|
||||
max_sep = float(cfg.get("max_hole_sep_px", 2000.0))
|
||||
if not (min_sep <= dist <= max_sep):
|
||||
return []
|
||||
picked[0].label = "left"
|
||||
picked[1].label = "right"
|
||||
return picked
|
||||
|
||||
|
||||
def _draw_holes(draw_img: np.ndarray, holes: list[HoleDetection]) -> None:
|
||||
for hole in holes:
|
||||
cx, cy = int(round(hole.center_px[0])), int(round(hole.center_px[1]))
|
||||
rr = max(4, int(round(hole.radius_px)))
|
||||
cv2.circle(draw_img, (cx, cy), rr, (0, 0, 255), 2)
|
||||
cv2.drawMarker(draw_img, (cx, cy), (0, 255, 255), cv2.MARKER_CROSS, 16, 2)
|
||||
cv2.circle(draw_img, (cx, cy), 3, (0, 255, 255), -1)
|
||||
name = hole.label or "hole"
|
||||
cv2.putText(
|
||||
draw_img,
|
||||
f"{name} ({cx},{cy}) {hole.confidence:.2f}",
|
||||
(cx + 12, cy - 8),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.55,
|
||||
(0, 0, 255),
|
||||
2,
|
||||
)
|
||||
|
||||
|
||||
def _finalize_holes(
|
||||
holes: list[HoleDetection],
|
||||
draw_img: np.ndarray,
|
||||
raw: dict[str, Any] | None = None,
|
||||
cfg: dict[str, Any] | None = None,
|
||||
) -> HoleDetectResult:
|
||||
cfg = cfg or {}
|
||||
paired = pair_two_holes(holes, cfg)
|
||||
if len(holes) >= 2 and not paired:
|
||||
_draw_holes(draw_img, holes)
|
||||
return HoleDetectResult(False, "两孔间距不在允许范围", holes, draw_img, raw=raw)
|
||||
if not paired:
|
||||
_draw_holes(draw_img, holes)
|
||||
return HoleDetectResult(False, f"有效孔数量={len(holes)}，需要 2 个", holes, draw_img, raw=raw)
|
||||
_draw_holes(draw_img, paired)
|
||||
dist = float(
|
||||
np.hypot(
|
||||
paired[1].center_px[0] - paired[0].center_px[0],
|
||||
paired[1].center_px[1] - paired[0].center_px[1],
|
||||
)
|
||||
)
|
||||
msg = f"检测到双孔 left={paired[0].center_px} right={paired[1].center_px} dist={dist:.1f}px"
|
||||
return HoleDetectResult(True, msg, paired, draw_img, raw=raw)
|
||||
|
||||
|
||||
def pixel_to_camera(
|
||||
hole: HoleDetection,
|
||||
depth_m: float,
|
||||
intrinsics: dict[str, float],
|
||||
) -> np.ndarray:
|
||||
"""像素坐标 + 深度 -> 相机系三维坐标（m）。"""
|
||||
fx = float(intrinsics["fx"])
|
||||
fy = float(intrinsics["fy"])
|
||||
cx = float(intrinsics["cx"])
|
||||
cy = float(intrinsics["cy"])
|
||||
u, v = hole.center_px
|
||||
z = float(depth_m)
|
||||
x = (u - cx) * z / fx
|
||||
y = (v - cy) * z / fy
|
||||
return np.array([x, y, z], dtype=float)
|
||||
@@ -0,0 +1,149 @@
|
||||
"""把 numpy 数组打包成 ``sensor_msgs/Image``。
|
||||
|
||||
为什么需要
|
||||
----------
|
||||
Orisys 视触觉传感器的数据分两类：
|
||||
|
||||
* **三力**（``fnormal`` / ``fshearx`` / ``fsheary``）—— 每帧几个标量，走子进程
|
||||
队列几乎没有开销。
|
||||
* **图像类**（形变图 / 深度图 / 力矢量场）—— 每帧几百 KB 到几 MB。原先
|
||||
``tactile_worker`` 为了性能**故意不过队列**，于是这些数据从未离开子进程、
|
||||
也从未发布到 ROS 话题上。
|
||||
|
||||
本模块只负责最后一公里：把已经拿到的数组正确打包成 ``Image``。放在单独文件
|
||||
里是为了能脱离相机单测 —— 编码、``step``、通道顺序这些细节一旦写错，下游
|
||||
拿到的图会静默错位（图像撕裂、颜色通道互换），而不是报错。
|
||||
|
||||
支持的形状与编码
|
||||
----------------
|
||||
=============== ============== ==========================================
|
||||
形状 dtype 编码
|
||||
=============== ============== ==========================================
|
||||
``(H, W)`` ``uint8`` ``mono8``
|
||||
``(H, W)`` ``uint16`` ``mono16``
|
||||
``(H, W)`` ``float32`` ``32FC1``
|
||||
``(H, W, 1)`` 任意 ``mono8``
|
||||
``(H, W, 2)`` ``float32`` ``32FC2``（力矢量场：每像素 dx, dy）
|
||||
``(H, W, 3)`` ``uint8`` ``rgb8``
|
||||
``(H, W, 4)`` ``uint8`` ``rgba8``
|
||||
=============== ============== ==========================================
|
||||
|
||||
2 通道的 ``32FC2`` 是 ROS 里表达矢量场的标准做法，``step = W * 4 * 2``。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
__all__ = ["ndarray_to_image", "infer_encoding", "UnsupportedImageError"]
|
||||
|
||||
|
||||
class UnsupportedImageError(ValueError):
|
||||
"""数组的形状/类型没有对应的 ROS 编码。
|
||||
|
||||
宁可显式报错也不要猜 —— 猜错的结果是下游拿到一张静默错位的图。
|
||||
错误消息里带上实际形状与 dtype，方便对照本文档的表格。
|
||||
"""
|
||||
|
||||
|
||||
#: (通道数, dtype) -> ROS 编码
|
||||
_ENCODING_BY_CHANNELS = {
|
||||
(1, np.dtype(np.uint8)): "mono8",
|
||||
(1, np.dtype(np.uint16)): "mono16",
|
||||
(1, np.dtype(np.float32)): "32FC1",
|
||||
(2, np.dtype(np.float32)): "32FC2",
|
||||
(3, np.dtype(np.uint8)): "rgb8",
|
||||
(4, np.dtype(np.uint8)): "rgba8",
|
||||
}
|
||||
|
||||
|
||||
def infer_encoding(array: np.ndarray) -> str:
|
||||
"""推断 ROS 编码。推不出来时抛 :class:`UnsupportedImageError`。"""
|
||||
arr = np.asarray(array)
|
||||
if arr.ndim == 2:
|
||||
channels = 1
|
||||
elif arr.ndim == 3:
|
||||
channels = arr.shape[2]
|
||||
else:
|
||||
raise UnsupportedImageError(
|
||||
f"只支持 2D（灰度）或 3D（H,W,C）数组，收到 {arr.ndim}D，形状 {arr.shape}"
|
||||
)
|
||||
|
||||
key = (channels, arr.dtype)
|
||||
encoding = _ENCODING_BY_CHANNELS.get(key)
|
||||
if encoding is None:
|
||||
supported = ", ".join(
|
||||
f"{enc}({c}通道/{d.name})"
|
||||
for (c, d), enc in sorted(_ENCODING_BY_CHANNELS.items(), key=lambda kv: kv[1])
|
||||
)
|
||||
raise UnsupportedImageError(
|
||||
f"没有 {channels} 通道 + {arr.dtype.name} 对应的 ROS 编码。"
|
||||
f"支持: {supported}。"
|
||||
f"（收到形状 {arr.shape}）"
|
||||
)
|
||||
return encoding
|
||||
|
||||
|
||||
def _layout(array: np.ndarray) -> Tuple[int, int, int]:
|
||||
"""返回 ``(height, width, channels)``，并把 (H,W,1) 归一化成 2D 语义。"""
|
||||
arr = np.asarray(array)
|
||||
if arr.ndim == 2:
|
||||
h, w = arr.shape
|
||||
return int(h), int(w), 1
|
||||
h, w, c = arr.shape
|
||||
return int(h), int(w), int(c)
|
||||
|
||||
|
||||
def ndarray_to_image(
|
||||
array: np.ndarray,
|
||||
*,
|
||||
frame_id: str = "",
|
||||
stamp=None,
|
||||
encoding: Optional[str] = None,
|
||||
) -> Image:
|
||||
"""把数组打包成 ``sensor_msgs/Image``。
|
||||
|
||||
参数
|
||||
----
|
||||
array:
|
||||
图像或矢量场。必须是 C 连续（``np.ascontiguousarray`` 会自动处理）。
|
||||
encoding:
|
||||
显式指定编码以覆盖推断。仅在确实知道自己在做什么时使用 ——
|
||||
编码与 dtype 不匹配会让下游按错误的元素大小解读 ``data``。
|
||||
"""
|
||||
arr = np.asarray(array)
|
||||
if encoding is None:
|
||||
encoding = infer_encoding(arr)
|
||||
|
||||
height, width, channels = _layout(arr)
|
||||
|
||||
# (H,W,1) 在内存里与 (H,W) 连续一致，压平即可。
|
||||
flat = np.ascontiguousarray(arr).reshape(-1)
|
||||
|
||||
msg = Image()
|
||||
if stamp is not None:
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = frame_id
|
||||
msg.height = height
|
||||
msg.width = width
|
||||
msg.encoding = encoding
|
||||
msg.is_bigendian = False
|
||||
msg.step = int(width * channels * flat.dtype.itemsize)
|
||||
msg.data = flat.tobytes()
|
||||
return msg
|
||||
|
||||
|
||||
def expected_step(array: np.ndarray, encoding: Optional[str] = None) -> int:
|
||||
"""算出应当写入 ``Image.step`` 的值。测试与自检用。"""
|
||||
arr = np.asarray(array)
|
||||
_, width, channels = _layout(arr)
|
||||
if encoding is not None:
|
||||
channels = {"mono8": 1, "mono16": 1, "32FC1": 1, "32FC2": 2,
|
||||
"rgb8": 3, "rgba8": 4}.get(encoding, channels)
|
||||
itemsize = {"mono8": 1, "mono16": 2, "32FC1": 4, "32FC2": 4,
|
||||
"rgb8": 1, "rgba8": 1}.get(encoding, arr.dtype.itemsize)
|
||||
return int(width * channels * itemsize)
|
||||
return int(width * channels * arr.dtype.itemsize)
|
||||
@@ -0,0 +1,97 @@
|
||||
#!/usr/bin/env python3
|
||||
"""三维点的匀速模型卡尔曼滤波（坐标系为相机系）。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class KalmanFilter3D:
|
||||
"""状态量为 ``[x, y, z, vx, vy, vz]``；观测只有位置，没有速度。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
q_pos: float = 1e-4,
|
||||
q_vel: float = 5e-3,
|
||||
r_pos: float = 4e-4,
|
||||
) -> None:
|
||||
self.q_pos = float(q_pos)
|
||||
self.q_vel = float(q_vel)
|
||||
self.r_pos = float(r_pos)
|
||||
self.x = np.zeros(6, dtype=float)
|
||||
self.P = np.eye(6, dtype=float)
|
||||
self._t: Optional[float] = None
|
||||
self.initialized = False
|
||||
|
||||
def reset(self, p: np.ndarray, t: Optional[float] = None) -> None:
|
||||
p = np.asarray(p, dtype=float).reshape(3)
|
||||
self.x[:] = 0.0
|
||||
self.x[:3] = p
|
||||
self.P = np.diag([1e-3, 1e-3, 1e-3, 1e-2, 1e-2, 1e-2])
|
||||
self._t = t
|
||||
self.initialized = True
|
||||
|
||||
@property
|
||||
def position(self) -> np.ndarray:
|
||||
return self.x[:3].copy()
|
||||
|
||||
@property
|
||||
def velocity(self) -> np.ndarray:
|
||||
return self.x[3:].copy()
|
||||
|
||||
def advance(self, t: float) -> None:
|
||||
"""把状态预测到时刻 ``t``（秒，必须单调递增）。
|
||||
|
||||
超过 0.5 秒的空档按"目标可能已经跑掉"处理：位置不动、速度清零，
|
||||
否则旧速度会把位置外推得很离谱。
|
||||
"""
|
||||
if not self.initialized or self._t is None:
|
||||
self._t = t
|
||||
return
|
||||
dt = float(t - self._t)
|
||||
if dt <= 1e-6:
|
||||
return
|
||||
if dt > 0.5:
|
||||
# 空档太长——位置保持不动，速度清零。
|
||||
self.x[3:] = 0.0
|
||||
self._t = t
|
||||
return
|
||||
self._predict(dt)
|
||||
self._t = t
|
||||
|
||||
def zero_velocity(self) -> None:
|
||||
"""把速度状态清零、位置保持不动。
|
||||
|
||||
用途：观测疑似假跳变（见 ``feature_jump_guard.FeatureJumpGate``）时本次不
|
||||
采信观测，但**不能**让上一次的速度继续把位置外推出去 —— 否则"不动"会变成
|
||||
"按旧速度匀速跑掉"，刚好是我们要避免的那件事。
|
||||
"""
|
||||
self.x[3:] = 0.0
|
||||
|
||||
def update(self, z: np.ndarray) -> None:
|
||||
"""用一次三维位置观测做量测更新。"""
|
||||
z = np.asarray(z, dtype=float).reshape(3)
|
||||
if not self.initialized:
|
||||
self.reset(z)
|
||||
return
|
||||
H = np.zeros((3, 6), dtype=float)
|
||||
H[0, 0] = H[1, 1] = H[2, 2] = 1.0
|
||||
R = np.eye(3, dtype=float) * self.r_pos
|
||||
y = z - H @ self.x
|
||||
S = H @ self.P @ H.T + R
|
||||
K = self.P @ H.T @ np.linalg.inv(S)
|
||||
self.x = self.x + K @ y
|
||||
I = np.eye(6)
|
||||
self.P = (I - K @ H) @ self.P
|
||||
|
||||
def _predict(self, dt: float) -> None:
|
||||
F = np.eye(6, dtype=float)
|
||||
F[0, 3] = F[1, 4] = F[2, 5] = dt
|
||||
q_p = self.q_pos
|
||||
q_v = self.q_vel
|
||||
Q = np.diag([q_p, q_p, q_p, q_v, q_v, q_v])
|
||||
self.x = F @ self.x
|
||||
self.P = F @ self.P @ F.T + Q
|
||||
@@ -0,0 +1,558 @@
|
||||
"""模型类别标签（class names）的统一读写。
|
||||
|
||||
## 为什么需要这个模块
|
||||
|
||||
推理代码需要把模型输出的**类别索引**（整数 0/1/2…）翻译成**人能看懂的名字**。
|
||||
这件事原先没有统一做法，导致同一个模型在不同后端下名字都不一样：
|
||||
|
||||
- `.pt`：从 Ultralytics 的 `model.names` 读（来源是 checkpoint 内嵌 metadata）
|
||||
- `.onnx`：ONNX 文件里有 `names` metadata，但代码没用，而是**硬编码** `{0: "workpiece"}`
|
||||
- `.engine`：**标签直接丢了**
|
||||
|
||||
最后一条是本模块存在的主要原因。TensorRT engine 要用 `trt.OnnxParser` 重新构建，
|
||||
而**当前安装的 TensorRT（10.13）Python API 根本没有 metadata 读写接口** ——
|
||||
`INetworkDefinition` / `IBuilderConfig` / `ICudaEngine` 上都没有任何 metadata 方法。
|
||||
所以 engine 构建完就只剩张量，标签无从附着。实测：
|
||||
|
||||
detect_best_0.engine 二进制里搜不到 "workpiece" → 标签已丢
|
||||
detect_hc90.engine 能搜到 "workpiece" → 旧版 TRT 建的，但当前环境已无法加载
|
||||
|
||||
（`detect_hc90` 已作为零引用模型移出仓库，这里保留这行是因为它同时印证了两件事：
|
||||
旧版 TRT 建的 engine 里其实是有标签的，以及 engine 与 TRT 版本绑死。）
|
||||
|
||||
后果很隐蔽：`yolo_detector` 读不到名字时会退化成 `str(cid)`，也就是类名变成字符串
|
||||
`"0"`；而预览界面又不绘制类名，所以**没有任何地方看得出标签丢了**。
|
||||
|
||||
## 解决办法：旁挂标签文件
|
||||
|
||||
在每个权重旁边放一个同名文本文件 `<模型名>.labels.yaml`：
|
||||
|
||||
models/yolo/detect_best_0.onnx
|
||||
models/yolo/detect_best_0.engine
|
||||
models/yolo/detect_best_0.labels.yaml ← 就是这个
|
||||
|
||||
选文本文件而不是想办法塞进 engine，理由有二：
|
||||
|
||||
1. **可修改**。标签是配置，不是模型的一部分。塞进 engine 就只能重建 engine 才能改。
|
||||
2. **跨格式统一**。engine / onnx / pt 三种格式共用同一份标签，不会出现
|
||||
"换后端名字就变了"。engine 丢标签的问题也一并绕过了。
|
||||
|
||||
标签文件是纯文本，进 git（体积小、属于配置）；权重二进制不进 git。
|
||||
|
||||
## 读取优先级
|
||||
|
||||
sidecar（旁挂文件） > onnx（ONNX metadata） > model（模型自身 metadata） > placeholder
|
||||
|
||||
`read_labels()` 会连**来源**一起返回。这点很重要：调用方可以据此提示
|
||||
"标签来自占位符，请补一个 .labels.yaml"，而不是悄悄给出一个错的类名。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Sequence, Tuple, Union
|
||||
|
||||
# 标签文件的扩展名。刻意用双后缀：既能被 *.yaml 工具识别，
|
||||
# 又能和模型自身的配置（如果有）区分开。
|
||||
LABELS_SUFFIX = ".labels.yaml"
|
||||
|
||||
# read_labels() 的来源标记。顺序即优先级（高 → 低）。
|
||||
SOURCE_SIDECAR = "sidecar" # 旁挂 <模型名>.labels.yaml（权威，用户可直接改）
|
||||
SOURCE_ONNX = "onnx" # ONNX 文件内嵌 metadata
|
||||
SOURCE_MODEL = "model" # 模型自身 metadata（Ultralytics 的 .names）
|
||||
SOURCE_PLACEHOLDER = "placeholder" # 都没有 → 占位符 class_0/class_1…
|
||||
|
||||
SOURCE_LABELS_ZH = {
|
||||
SOURCE_SIDECAR: "旁挂标签文件",
|
||||
SOURCE_ONNX: "ONNX 内嵌 metadata",
|
||||
SOURCE_MODEL: "模型内嵌 metadata",
|
||||
SOURCE_PLACEHOLDER: "占位符（缺标签）",
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class LabelSet:
|
||||
"""一次标签读取的结果。
|
||||
|
||||
``names`` 是索引 → 名字；``source`` 说明它从哪来（见 SOURCE_* 常量）。
|
||||
|
||||
调用方**应该**检查 ``source``：如果是 ``placeholder``，说明这个模型缺标签，
|
||||
此时给出的是 ``class_0`` 这样的占位名，只适合占位显示，不该当成真名使用。
|
||||
"""
|
||||
|
||||
names: Dict[int, str]
|
||||
source: str
|
||||
path: Optional[Path] = None # 实际读到的文件（placeholder 时为 None）
|
||||
|
||||
@property
|
||||
def is_placeholder(self) -> bool:
|
||||
return self.source == SOURCE_PLACEHOLDER
|
||||
|
||||
def get(self, cid: int, default: Optional[str] = None) -> str:
|
||||
if cid in self.names:
|
||||
return self.names[cid]
|
||||
return default if default is not None else f"class_{cid}"
|
||||
|
||||
def sorted_names(self) -> List[str]:
|
||||
return [self.names[k] for k in sorted(self.names)]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 路径
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def labels_path_for(weights: Union[str, Path]) -> Path:
|
||||
"""给出权重对应的旁挂标签文件路径。
|
||||
|
||||
只换后缀，目录不变：``models/yolo/detect_best_0.engine`` →
|
||||
``models/yolo/detect_best_0.labels.yaml``。
|
||||
|
||||
这样无论 ``resolve_weights()`` 最后选中了 .pt/.onnx/.engine 中的哪一个，
|
||||
算出来的标签路径都相同 —— 一份标签服务同一个模型的所有格式。
|
||||
"""
|
||||
p = Path(weights).expanduser()
|
||||
# 用 stem 手工拼接，而不是 with_suffix('.labels.yaml')：
|
||||
# 后者对 `a.b.onnx` 这类含点文件名容易踩坑，手工拼没有歧义。
|
||||
return p.parent / (p.stem + LABELS_SUFFIX)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 解析：把各种形状的标签定义统一成 {索引: 名字}
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _clean(text: object) -> str:
|
||||
# None 必须先拦掉：str(None) == "None" 是 truthy 的，
|
||||
# 直接 str() 会让空项变成一个字面量为 "None" 的类名。
|
||||
if text is None:
|
||||
return ""
|
||||
return str(text).strip()
|
||||
|
||||
|
||||
def _try_parse_structured(text: str) -> Optional[Dict[int, str]]:
|
||||
"""尝试把字符串当成 dict/list 的字面量解析。
|
||||
|
||||
为什么需要：ONNX metadata 里的 ``names`` 存的**不是** JSON，也不是逗号列表，
|
||||
而是 Python 字典的 repr（单引号）：
|
||||
|
||||
"{0: 'workpiece'}"
|
||||
|
||||
Ultralytics 是直接 ``str(names)`` 写进去的，所以 ``json.loads`` 会失败。
|
||||
两种都试一遍（JSON 标准、literal_eval 兜底），都不行才交给逗号解析。
|
||||
``ast.literal_eval`` 只解析字面量、不执行代码，是安全的。
|
||||
"""
|
||||
s = text.strip()
|
||||
if not s:
|
||||
return None
|
||||
import ast
|
||||
import json
|
||||
|
||||
for loader in (json.loads, ast.literal_eval):
|
||||
try:
|
||||
obj = loader(s)
|
||||
except Exception: # noqa: BLE001
|
||||
continue
|
||||
if isinstance(obj, (dict, list, tuple)):
|
||||
return parse_names_field(obj)
|
||||
return None
|
||||
|
||||
|
||||
def _parse_freeform_text(text: str) -> Dict[int, str]:
|
||||
"""解析"自由文本"形式的标签内容。
|
||||
|
||||
两种情况要分开，否则会互相吃掉：
|
||||
|
||||
- 一行一个名字（有人把 ``classes.txt`` 直接改名成 ``.labels.yaml``）：
|
||||
``"a\\nb"`` → ``{0:'a', 1:'b'}``
|
||||
- 单行逗号串：``"a, b"`` → ``{0:'a', 1:'b'}``
|
||||
|
||||
判据是"有没有多行"。只有一行时才按逗号切，
|
||||
否则 ``"a, b"`` 这种文件名里带逗号的写法会被切错。
|
||||
"""
|
||||
items = [
|
||||
ln.strip()
|
||||
for ln in text.splitlines()
|
||||
if ln.strip() and not ln.strip().startswith("#")
|
||||
]
|
||||
if len(items) > 1:
|
||||
return {i: n for i, n in enumerate(items)}
|
||||
return parse_label_list(text)
|
||||
|
||||
|
||||
def parse_names_field(value: object) -> Dict[int, str]:
|
||||
"""把 YAML/metadata 里的 ``names`` 字段规范成 ``{int: str}``。
|
||||
|
||||
容忍三种写法，因为它们各自都很常见：
|
||||
|
||||
- 字典（Ultralytics data.yaml 的写法）：``{0: workpiece, 1: hole}``
|
||||
- 列表（本模块写出的规范形式，索引即顺序）：``[workpiece, hole]``
|
||||
- 逗号分隔的字符串：``"workpiece, hole"``
|
||||
|
||||
跳过空项但**保留其索引位置**，这样 ``[a, "", c]`` 不会让 c 变成 1 号。
|
||||
无法解析成整数键的字典项会被忽略（不抛异常 —— 标签读不出来不该让程序崩）。
|
||||
"""
|
||||
if value is None:
|
||||
return {}
|
||||
if isinstance(value, dict):
|
||||
out: Dict[int, str] = {}
|
||||
for k, v in value.items():
|
||||
try:
|
||||
idx = int(k)
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
name = _clean(v)
|
||||
if name:
|
||||
out[idx] = name
|
||||
return out
|
||||
if isinstance(value, (list, tuple)):
|
||||
out = {}
|
||||
for i, v in enumerate(value):
|
||||
name = _clean(v)
|
||||
if name:
|
||||
out[i] = name
|
||||
return out
|
||||
if isinstance(value, str):
|
||||
# 字符串可能是三种东西：dict/list 的字面量（ONNX metadata）、
|
||||
# 逗号列表、或者多行文本。先试结构化的，再退到逗号/多行解析。
|
||||
structured = _try_parse_structured(value)
|
||||
if structured is not None:
|
||||
return structured
|
||||
return _parse_freeform_text(value)
|
||||
return {}
|
||||
|
||||
|
||||
def parse_label_list(text: str) -> Dict[int, str]:
|
||||
"""解析逗号（或中文逗号、分号）分隔的类别名。
|
||||
|
||||
``"workpiece, hole"`` → ``{0: 'workpiece', 1: 'hole'}``
|
||||
|
||||
同时兼容 ``"0:workpiece, 1:hole"`` 这种带显式索引的写法，
|
||||
因为用户从别处拷配置过来时经常带着索引。空项被跳过。
|
||||
"""
|
||||
if not text or not text.strip():
|
||||
return {}
|
||||
parts = [p for p in re.split(r"[,，;；\n]", text)]
|
||||
out: Dict[int, str] = {}
|
||||
next_idx = 0
|
||||
for raw in parts:
|
||||
item = raw.strip()
|
||||
if not item:
|
||||
continue
|
||||
m = re.match(r"^\s*(\d+)\s*[:：]\s*(.+)$", item)
|
||||
if m:
|
||||
idx = int(m.group(1))
|
||||
name = m.group(2).strip()
|
||||
if name:
|
||||
out[idx] = name
|
||||
next_idx = max(next_idx, idx + 1)
|
||||
continue
|
||||
out[next_idx] = item
|
||||
next_idx += 1
|
||||
return out
|
||||
|
||||
|
||||
def read_label_text_file(path: Union[str, Path]) -> Dict[int, str]:
|
||||
"""读纯文本标签文件：一行一个类名（YOLO 的 classes.txt 就是这个格式）。
|
||||
|
||||
空行和 ``#`` 开头的注释行被忽略，且**不占索引**。
|
||||
这样用户可以在文件里随意加注释而不破坏索引对应关系。
|
||||
"""
|
||||
p = Path(path).expanduser()
|
||||
out: Dict[int, str] = {}
|
||||
idx = 0
|
||||
with p.open("r", encoding="utf-8") as f:
|
||||
for line in f:
|
||||
s = line.strip()
|
||||
if not s or s.startswith("#"):
|
||||
continue
|
||||
out[idx] = s
|
||||
idx += 1
|
||||
return out
|
||||
|
||||
|
||||
def placeholder_labels(nc: int) -> Dict[int, str]:
|
||||
"""没有标签可用时的占位名。
|
||||
|
||||
刻意用 ``class_0`` 而不是直接回退成字符串形式的数字 ``"0"``：
|
||||
后者和"真的有一个叫 0 的类"无法区分，正是原先难以发现问题所在。
|
||||
``class_`` 前缀一眼就能看出是没配标签。
|
||||
"""
|
||||
n = max(0, int(nc))
|
||||
return {i: f"class_{i}" for i in range(n)}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 旁挂标签文件读写
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def format_labels_yaml(names: Dict[int, str], model: Optional[str] = None) -> str:
|
||||
"""生成旁挂标签文件的内容（带使用说明的注释头）。
|
||||
|
||||
注释头是刻意保留的：这个文件会被不熟悉的人直接打开编辑，
|
||||
把"第几行 = 第几类"写在文件里比写在文档里有效得多。
|
||||
"""
|
||||
lines: List[str] = []
|
||||
lines.append("# 模型类别标签（class names）")
|
||||
lines.append("#")
|
||||
lines.append("# 一个模型对应一份标签，放在权重旁边、同名前缀：")
|
||||
lines.append("# detect_best_0.engine ← 权重")
|
||||
lines.append("# detect_best_0.labels.yaml ← 本文件")
|
||||
lines.append("#")
|
||||
lines.append("# 写法：names 下面一行一个类名，第 1 行对应类别 0，第 2 行对应类别 1，依次类推。")
|
||||
lines.append("# 改完保存即生效，不需要重新转换模型、也不用重启环境（重跑节点即可）。")
|
||||
lines.append("#")
|
||||
lines.append("# 命令行查看/修改（推荐，不用手改本文件）：")
|
||||
lines.append("# bash ~/ros2/scripts/weights.sh labels <模型名>")
|
||||
lines.append("# bash ~/ros2/scripts/weights.sh labels <模型名> --set \"工件,通止规\"")
|
||||
lines.append("")
|
||||
if model:
|
||||
# 记下所属模型，便于发现"标签文件被拷给了另一个模型"这种错配。
|
||||
lines.append(f"model: {model}")
|
||||
lines.append("names:")
|
||||
if names:
|
||||
for k in sorted(names):
|
||||
lines.append(f" - {names[k]}")
|
||||
else:
|
||||
lines.append(" []")
|
||||
return "\n".join(lines) + "\n"
|
||||
|
||||
|
||||
def write_labels(
|
||||
weights: Union[str, Path],
|
||||
names: Dict[int, str],
|
||||
*,
|
||||
path: Optional[Union[str, Path]] = None,
|
||||
) -> Path:
|
||||
"""把标签写到权重旁边的旁挂文件，返回写入路径。"""
|
||||
dst = Path(path).expanduser() if path else labels_path_for(weights)
|
||||
dst.parent.mkdir(parents=True, exist_ok=True)
|
||||
model = Path(weights).stem
|
||||
dst.write_text(format_labels_yaml(names, model=model), encoding="utf-8")
|
||||
return dst
|
||||
|
||||
|
||||
def read_label_file(path: Union[str, Path]) -> Optional[Dict[int, str]]:
|
||||
"""读旁挂标签文件。文件不存在或内容为空返回 None（不抛异常）。"""
|
||||
p = Path(path).expanduser()
|
||||
if not p.is_file():
|
||||
return None
|
||||
try:
|
||||
raw = p.read_text(encoding="utf-8")
|
||||
except Exception: # noqa: BLE001
|
||||
return None
|
||||
try:
|
||||
import yaml
|
||||
|
||||
data = yaml.safe_load(raw)
|
||||
except Exception: # noqa: BLE001
|
||||
# 文件存在但读不动（语法错、编码错、权限）。返回 None 让调用方按
|
||||
# "没有标签"处理，而不是让整个推理进程崩掉 —— 标签缺失不该是致命的。
|
||||
return None
|
||||
|
||||
if isinstance(data, dict):
|
||||
# 规范形式：names: [...]
|
||||
names = parse_names_field(data.get("names"))
|
||||
if names:
|
||||
return names
|
||||
# 容忍 {0: 'a', 1: 'b'} 这种"顶层直接就是索引字典"的写法（Ultralytics data.yaml）
|
||||
names = {
|
||||
int(k): str(v).strip()
|
||||
for k, v in data.items()
|
||||
if str(k).lstrip("-").isdigit() and str(v).strip()
|
||||
}
|
||||
# 是合法 YAML 字典却读不出类别名 → 就是没有标签。
|
||||
# 这里**不能**再回落到"按行读文件"，否则会把 `names: []` 里的
|
||||
# 'names:' 这几个字当成类名。
|
||||
return names or None
|
||||
|
||||
if isinstance(data, (list, tuple)):
|
||||
return parse_names_field(data) or None
|
||||
|
||||
if isinstance(data, str):
|
||||
# 整个文件就是一个标量 —— 可能是"改名过来的 classes.txt"。
|
||||
# 注意必须用**原始文本**：YAML 会把多行纯标量折成一行
|
||||
# （"a\nb\n" → "a b"），用 data 的话换行已经没了，
|
||||
# "一行一个类名"会被当成一个带空格的长名字。
|
||||
return _parse_freeform_text(raw) or None
|
||||
|
||||
# 空文件 / 只有注释 / 其它标量类型：按行读（能正确处理注释行）。
|
||||
try:
|
||||
return read_label_text_file(p) or None
|
||||
except Exception: # noqa: BLE001
|
||||
return None
|
||||
|
||||
|
||||
def label_file_model(path: Union[str, Path]) -> Optional[str]:
|
||||
"""读旁挂标签文件里记录的 ``model:`` 字段（用于发现标签错配）。"""
|
||||
p = Path(path).expanduser()
|
||||
if not p.is_file():
|
||||
return None
|
||||
try:
|
||||
import yaml
|
||||
|
||||
data = yaml.safe_load(p.read_text(encoding="utf-8"))
|
||||
except Exception: # noqa: BLE001
|
||||
return None
|
||||
if isinstance(data, dict):
|
||||
v = data.get("model")
|
||||
return _clean(v) if v else None
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 从模型自身读标签
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def onnx_metadata(path: Union[str, Path]) -> Dict[str, str]:
|
||||
"""读 ONNX 的 ``metadata_props``（全是字符串），读不到返回空字典。
|
||||
|
||||
用 ``load_external_data=False``：只读图结构不读权重，对 38MB 的模型也是毫秒级。
|
||||
Ultralytics 导出时会写入 ``names`` / ``imgsz`` / ``nc`` / ``task`` 等。
|
||||
"""
|
||||
p = Path(path).expanduser()
|
||||
if not p.is_file():
|
||||
return {}
|
||||
try:
|
||||
import onnx
|
||||
|
||||
m = onnx.load(str(p), load_external_data=False)
|
||||
except Exception: # noqa: BLE001
|
||||
return {}
|
||||
return {x.key: x.value for x in m.metadata_props}
|
||||
|
||||
|
||||
def labels_from_onnx(path: Union[str, Path]) -> Optional[Tuple[Dict[int, str], int]]:
|
||||
"""从 ONNX 的 ``metadata_props`` 读 ``names`` 与类别数 ``nc``。
|
||||
|
||||
返回 ``(names, nc)``；读不到返回 None。
|
||||
|
||||
注意 ``names`` 在 ONNX 里存的是 **Python 字典的 repr 字符串**（单引号），
|
||||
形如 ``"{0: 'workpiece'}"`` —— 不是 JSON。解析细节见 ``parse_names_field``。
|
||||
"""
|
||||
meta = onnx_metadata(path)
|
||||
if not meta:
|
||||
return None
|
||||
|
||||
names = parse_names_field(meta.get("names"))
|
||||
nc = 0
|
||||
raw_nc = meta.get("nc")
|
||||
if raw_nc is not None:
|
||||
try:
|
||||
nc = int(str(raw_nc).strip())
|
||||
except (TypeError, ValueError):
|
||||
nc = 0
|
||||
if not nc and names:
|
||||
nc = max(names) + 1
|
||||
return (names, nc) if names else None
|
||||
|
||||
|
||||
def labels_from_model(path: Union[str, Path]) -> Optional[Tuple[Dict[int, str], int]]:
|
||||
"""从模型自身 metadata 读标签（走 Ultralytics，主要给 .pt 用）。
|
||||
|
||||
对 .engine / .onnx 也能用（Ultralytics 会自己反序列化），但代价高：
|
||||
要加载整个模型。所以 ``read_labels()`` 只在必要时才调它。
|
||||
|
||||
Ultralytics 的 model 对象除了 ``names`` 还有 ``nc``；拿不到 nc 时用 names 推断。
|
||||
"""
|
||||
p = Path(path).expanduser()
|
||||
if not p.is_file():
|
||||
return None
|
||||
try:
|
||||
from ultralytics import YOLO
|
||||
|
||||
model = YOLO(str(p), task="detect")
|
||||
raw = getattr(model, "names", None) or {}
|
||||
names = parse_names_field(raw)
|
||||
nc = 0
|
||||
for attr in ("nc",):
|
||||
try:
|
||||
nc = int(getattr(model, attr) or 0)
|
||||
except Exception: # noqa: BLE001
|
||||
nc = 0
|
||||
if not nc and names:
|
||||
nc = max(names) + 1
|
||||
return (names, nc) if names else None
|
||||
except Exception: # noqa: BLE001
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 统一入口
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def read_labels(
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
allow_model_metadata: bool = True,
|
||||
nc_hint: int = 0,
|
||||
) -> LabelSet:
|
||||
"""按优先级读出一个模型该用的标签。
|
||||
|
||||
优先级：旁挂文件 > ONNX metadata > 模型自身 metadata > 占位符。
|
||||
|
||||
旁挂优先是刻意的：它是用户能直接编辑的那一份，必须覆盖模型内嵌的旧值，
|
||||
否则"我明明改了标签为什么不生效"就会变成一个很难查的问题。
|
||||
|
||||
绝不抛异常。读不到就返回占位符并把原因记在 ``source`` 里，
|
||||
由调用方决定是记一条警告还是直接显示。
|
||||
"""
|
||||
p = Path(weights).expanduser()
|
||||
|
||||
side = labels_path_for(p)
|
||||
names = read_label_file(side)
|
||||
if names:
|
||||
return LabelSet(names=names, source=SOURCE_SIDECAR, path=side)
|
||||
|
||||
suffix = p.suffix.lower()
|
||||
# .pt 的标签只可能来自模型自身；.onnx 先看内嵌 metadata（快）；
|
||||
# .engine / .trt 没有可用 metadata（见模块头说明），直接落到占位符或模型加载。
|
||||
if suffix == ".onnx":
|
||||
got = labels_from_onnx(p)
|
||||
if got:
|
||||
return LabelSet(names=got[0], source=SOURCE_ONNX, path=p)
|
||||
elif suffix in (".pt", ".pth"):
|
||||
got = labels_from_model(p)
|
||||
if got:
|
||||
return LabelSet(names=got[0], source=SOURCE_MODEL, path=p)
|
||||
|
||||
if allow_model_metadata and suffix in (".engine", ".trt"):
|
||||
# engine 一般读不到（当前 TRT 没 metadata API），但也别直接放弃：
|
||||
# 若用户是用 Ultralytics 导出的 engine，metadata 可能在。
|
||||
got = labels_from_model(p)
|
||||
if got:
|
||||
return LabelSet(names=got[0], source=SOURCE_MODEL, path=p)
|
||||
|
||||
return LabelSet(names=placeholder_labels(nc_hint), source=SOURCE_PLACEHOLDER, path=None)
|
||||
|
||||
|
||||
def describe(labels: LabelSet) -> str:
|
||||
"""一句人能看懂的说明，用于日志：``workpiece (来源: 旁挂标签文件)``。"""
|
||||
names = ", ".join(labels.sorted_names()) or "(空)"
|
||||
src = SOURCE_LABELS_ZH.get(labels.source, labels.source)
|
||||
return f"{names} (来源: {src})"
|
||||
|
||||
|
||||
def label_file_status(
|
||||
weights: Union[str, Path],
|
||||
) -> Tuple[bool, Optional[Path], Optional[str]]:
|
||||
"""检查旁挂标签文件状态，供 doctor 用。
|
||||
|
||||
返回 ``(存在, 路径, 提示)``；``提示`` 为 None 表示没有问题。
|
||||
"""
|
||||
p = Path(weights).expanduser()
|
||||
side = labels_path_for(p)
|
||||
if not side.is_file():
|
||||
return False, side, f"缺标签文件：{side.name}"
|
||||
recorded = label_file_model(side)
|
||||
if recorded and Path(weights).stem != recorded:
|
||||
return True, side, (
|
||||
f"标签文件记录的 model 是 '{recorded}'，但当前权重是 "
|
||||
f"'{Path(weights).stem}' —— 可能是拷错了标签文件"
|
||||
)
|
||||
return True, side, None
|
||||
@@ -0,0 +1,152 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Direct ONNX Runtime inference for Ultralytics end2end detect models.
|
||||
|
||||
为什么不走 ultralytics.YOLO：它对 .onnx 会按发行包名检查 onnxruntime，
|
||||
环境装的是 onnxruntime-gpu 时会触发 pip AutoUpdate（可能卡死/破坏环境）。
|
||||
本模块用 onnxruntime 直接推理，显式 CPU provider，一次性拍照场景足够快。
|
||||
|
||||
模型要求：Ultralytics 导出时 end2end=True（NMS 已融合），
|
||||
输出形状 [1, N, 6] = x1, y1, x2, y2, conf, cls（letterbox 坐标系）。
|
||||
detect_best_1.onnx 满足（metadata: end2end=True, nms=False）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Union
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.model_labels import (
|
||||
SOURCE_MODEL,
|
||||
LabelSet,
|
||||
parse_names_field,
|
||||
read_labels,
|
||||
)
|
||||
from arm_application.yolo_detector import Detection
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class OnnxWorkpieceDetector:
|
||||
"""给 detect_holes() 用的最小接口：detect(image, conf, iou, imgsz)。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
conf: float = 0.25,
|
||||
imgsz: int = 640,
|
||||
class_names: Optional[dict] = None,
|
||||
) -> None:
|
||||
import onnxruntime as ort
|
||||
|
||||
path = Path(weights).expanduser()
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"ONNX weights not found: {path}")
|
||||
so = ort.SessionOptions()
|
||||
so.log_severity_level = 3 # 关掉 ORT 刷屏告警
|
||||
self._sess = ort.InferenceSession(
|
||||
str(path), sess_options=so, providers=["CPUExecutionProvider"]
|
||||
)
|
||||
inp = self._sess.get_inputs()[0]
|
||||
self._iname = inp.name
|
||||
shape = [d if isinstance(d, int) and d > 0 else imgsz for d in inp.shape]
|
||||
self._in_h, self._in_w = int(shape[-2]), int(shape[-1])
|
||||
# 只认 ultralytics end2end 检测输出 ``[1, N, 6]``，否则**当场报错**。
|
||||
# 为什么必须在这里硬失败：2026-10-08 真机，把一个 ``task=pose`` 的模型
|
||||
# （输出 ``[1, 4+nc+3K, 8400]``）从这条分支解出来，得到的是 12 个垃圾框
|
||||
# ——conf 直接是坐标值（最高 70.87）、类名 ``class_73``、退化框 ``y1=y2=0``。
|
||||
# 而流水线照跑不误，只在二阶段"看不出工件"上体现出来，排查代价极大。
|
||||
# 宁可启动就失败（元规则：静默失效是头号敌人）。
|
||||
outs = self._sess.get_outputs()
|
||||
tail = outs[0].shape[-1] if (outs and outs[0].shape) else None
|
||||
if isinstance(tail, int) and tail != 6:
|
||||
raise RuntimeError(
|
||||
f"{path.name} 的检测输出最后一维是 {tail}，不是 6 —— 它不是"
|
||||
" ultralytics end2end 检测模型（[1,N,6]）。带 k0/k1/k2 关键点的"
|
||||
" pose 模型必须交给 pose_detector / build_detector，否则坐标会被"
|
||||
"当成置信度解出垃圾框，而且不会报错。"
|
||||
)
|
||||
self.conf = float(conf)
|
||||
self.imgsz = int(imgsz)
|
||||
# 类别名：显式传入 > 旁挂标签文件 > ONNX 内嵌 metadata > 占位符。
|
||||
# 原先这里是硬编码 {0: "workpiece"}，等于把类名写死在代码里 ——
|
||||
# 换一个多类模型就会展示错误的名字，而且没人改得动。
|
||||
if class_names:
|
||||
names = parse_names_field(class_names)
|
||||
self._labels = LabelSet(names=names, source=SOURCE_MODEL, path=path)
|
||||
else:
|
||||
self._labels = read_labels(path, allow_model_metadata=False)
|
||||
if self._labels.is_placeholder:
|
||||
logger.warning(
|
||||
"ONNX %s 没有类别标签，将用占位名显示。"
|
||||
"补法: bash ~/ros2/scripts/weights.sh labels %s --from-model",
|
||||
path.name,
|
||||
path.stem,
|
||||
)
|
||||
# 兼容旧属性名，避免外部代码引用时报 AttributeError。
|
||||
self._names = dict(self._labels.names)
|
||||
|
||||
def _letterbox(self, img: np.ndarray):
|
||||
h, w = img.shape[:2]
|
||||
scale = min(self._in_w / w, self._in_h / h)
|
||||
nw, nh = int(round(w * scale)), int(round(h * scale))
|
||||
import cv2
|
||||
|
||||
resized = cv2.resize(img, (nw, nh), interpolation=cv2.INTER_LINEAR)
|
||||
canvas = np.full((self._in_h, self._in_w, 3), 114, dtype=np.uint8)
|
||||
top = (self._in_h - nh) // 2
|
||||
left = (self._in_w - nw) // 2
|
||||
canvas[top : top + nh, left : left + nw] = resized
|
||||
return canvas, scale, left, top
|
||||
|
||||
def detect(
|
||||
self,
|
||||
image_bgr: np.ndarray,
|
||||
conf: Optional[float] = None,
|
||||
iou: Optional[float] = None, # end2end 模型已内置 NMS，忽略
|
||||
imgsz: Optional[int] = None, # 输入尺寸由模型决定，忽略
|
||||
**_kwargs,
|
||||
) -> List[Detection]:
|
||||
del iou, imgsz
|
||||
thr = float(conf if conf is not None else self.conf)
|
||||
canvas, scale, left, top = self._letterbox(image_bgr)
|
||||
blob = canvas[:, :, ::-1].transpose(2, 0, 1)[None].astype(np.float32) / 255.0
|
||||
out = self._sess.run(None, {self._iname: blob})[0] # [1, N, 6]
|
||||
dets: List[Detection] = []
|
||||
if out is None or out.size == 0:
|
||||
return dets
|
||||
arr = np.asarray(out).reshape(-1, out.shape[-1])
|
||||
h, w = image_bgr.shape[:2]
|
||||
for row in arr:
|
||||
if row.shape[0] < 6:
|
||||
continue
|
||||
x1, y1, x2, y2, score, cls = [float(v) for v in row[:6]]
|
||||
if score < thr:
|
||||
continue
|
||||
# letterbox 坐标 → 原图坐标
|
||||
xa = (x1 - left) / scale
|
||||
ya = (y1 - top) / scale
|
||||
xb = (x2 - left) / scale
|
||||
yb = (y2 - top) / scale
|
||||
xyxy = np.array(
|
||||
[
|
||||
float(np.clip(xa, 0, w - 1)),
|
||||
float(np.clip(ya, 0, h - 1)),
|
||||
float(np.clip(xb, 0, w - 1)),
|
||||
float(np.clip(yb, 0, h - 1)),
|
||||
]
|
||||
)
|
||||
cid = int(cls)
|
||||
dets.append(
|
||||
Detection(
|
||||
xyxy=xyxy,
|
||||
conf=float(score),
|
||||
cls_id=cid,
|
||||
cls_name=self._labels.get(cid),
|
||||
)
|
||||
)
|
||||
dets.sort(key=lambda d: d.conf, reverse=True)
|
||||
return dets
|
||||
@@ -0,0 +1,587 @@
|
||||
"""Orisys 视触觉适配器。SDK 通过 pip 安装；设备 id 存放在 ``site.yaml`` 里。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.site_io import ros2_root
|
||||
from arm_application.tactile_interface import TactileCommand, TactileInterface, TactileSample
|
||||
|
||||
|
||||
@dataclass
|
||||
class OrisysSensorConfig:
|
||||
vid_src: int | str = 0
|
||||
config_name: str = "dd03"
|
||||
rotation_config_path: str = ""
|
||||
cuda: bool = True
|
||||
isstitch: bool = True
|
||||
backend: str = "auto"
|
||||
motion_threshold: float = 0.0
|
||||
target_fps: float = 30.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class OrisysDualConfig:
|
||||
left: OrisysSensorConfig
|
||||
right: OrisysSensorConfig
|
||||
baseline_seconds: float = 2.0
|
||||
filter_alpha: float = 0.25
|
||||
backend: str = "orisys" # orisys | mock
|
||||
image_hz: float = 0.0
|
||||
"""视触觉图像（深度/形变图）的发布频率。0 = 不发布图像。
|
||||
|
||||
为什么默认关：图像每帧几百 KB，跨进程序列化 + ROS 发布的开销远大于三力。
|
||||
三力路径必须跑在 30fps（接触判据靠它），图像只是给人看和给算法备用，
|
||||
默认不开以免动到已经调好的三力时序。装配现场按需在 site.yaml 打开。"""
|
||||
|
||||
|
||||
def resolve_orisys_config(config_name: str, config_file: str = "") -> str:
|
||||
"""把型号名解析成实际的 config.json 路径。
|
||||
|
||||
查找顺序（先命中先用）：
|
||||
|
||||
1. 显式传入的 ``config_file``
|
||||
2. 参数本身就是存在的文件路径
|
||||
3. ``devices/sensors/orisys/models/<型号>/config.json`` —— 型号配置随 SDK 放，
|
||||
这是现在唯一推荐的位置
|
||||
4. 老位置 ``src/arm_application/config/orisys/<型号>.json``（兼容保留：
|
||||
站点自己放的定制配置可能还在这里）
|
||||
5. Orisys 包自带的 ``configs/<型号>.json``
|
||||
6. 都没找到就把原样返回，交给 SDK 自己解析（它支持内置型号名）
|
||||
"""
|
||||
if config_file:
|
||||
p = Path(config_file).expanduser()
|
||||
if p.is_file():
|
||||
return str(p)
|
||||
raw = (config_name or "dd03").strip()
|
||||
p = Path(raw).expanduser()
|
||||
if p.is_file():
|
||||
return str(p)
|
||||
|
||||
root = ros2_root()
|
||||
for cand in (
|
||||
root / "devices" / "sensors" / "orisys" / "models" / raw / "config.json",
|
||||
root / "src" / "arm_application" / "config" / "orisys" / f"{raw}.json",
|
||||
):
|
||||
if cand.is_file():
|
||||
return str(cand)
|
||||
try:
|
||||
import orisys
|
||||
|
||||
pkg_root = Path(orisys.__file__).resolve().parent.parent
|
||||
for cand in (pkg_root / "configs" / f"{raw}.json", pkg_root / "config" / f"{raw}.json"):
|
||||
if cand.is_file():
|
||||
return str(cand)
|
||||
except Exception:
|
||||
pass
|
||||
return raw
|
||||
|
||||
|
||||
def load_orisys_from_site(data: dict) -> OrisysDualConfig:
|
||||
t = data.get("tactile") or {}
|
||||
left_raw = t.get("left") if isinstance(t.get("left"), dict) else {}
|
||||
right_raw = t.get("right") if isinstance(t.get("right"), dict) else {}
|
||||
cfg_file = str(t.get("config_file") or "")
|
||||
# model 是设备契约层选型号用的键（对应 models/<型号>/ 目录名）；
|
||||
# config 是它的旧名，仅为兼容老站点文件保留，站点里应统一用 model。
|
||||
cfg_name = str(t.get("model") or t.get("config") or "dd03")
|
||||
resolved = resolve_orisys_config(cfg_name, cfg_file)
|
||||
common = dict(
|
||||
config_name=resolved,
|
||||
rotation_config_path=str(t.get("rotation_config") or ""),
|
||||
cuda=bool(t.get("cuda", True)),
|
||||
isstitch=bool(t.get("isstitch", True)),
|
||||
backend=str(t.get("infer_backend") or t.get("backend_infer") or "auto"),
|
||||
motion_threshold=float(t.get("motion_threshold") or 0.0),
|
||||
target_fps=float(t.get("target_fps") or 30.0),
|
||||
)
|
||||
left_vid = left_raw.get("video", t.get("left_video", 0))
|
||||
right_vid = right_raw.get("video", t.get("right_video", 2))
|
||||
left = OrisysSensorConfig(vid_src=_parse_video(left_vid), **common)
|
||||
right = OrisysSensorConfig(vid_src=_parse_video(right_vid), **common)
|
||||
if left_raw.get("config") or left_raw.get("config_file"):
|
||||
left.config_name = resolve_orisys_config(
|
||||
str(left_raw.get("config") or cfg_name),
|
||||
str(left_raw.get("config_file") or ""),
|
||||
)
|
||||
if right_raw.get("config") or right_raw.get("config_file"):
|
||||
right.config_name = resolve_orisys_config(
|
||||
str(right_raw.get("config") or cfg_name),
|
||||
str(right_raw.get("config_file") or ""),
|
||||
)
|
||||
left.rotation_config_path = str(
|
||||
left_raw.get("rotation_config") or t.get("left_rotation") or t.get("rotation_config") or ""
|
||||
)
|
||||
right.rotation_config_path = str(
|
||||
right_raw.get("rotation_config") or t.get("right_rotation") or t.get("rotation_config") or ""
|
||||
)
|
||||
return OrisysDualConfig(
|
||||
left=left,
|
||||
right=right,
|
||||
baseline_seconds=float(t.get("baseline_seconds") or 2.0),
|
||||
filter_alpha=float(t.get("filter_alpha") or 0.25),
|
||||
backend=str(t.get("backend") or "orisys"),
|
||||
image_hz=float(t.get("image_hz") or 0.0),
|
||||
)
|
||||
|
||||
|
||||
def read_vector_map(sensor: Any) -> Optional[np.ndarray]:
|
||||
"""尝试从传感器读出力矢量场（每像素的切向力/位移矢量）。
|
||||
|
||||
为什么要"尝试"
|
||||
--------------
|
||||
Orisys 的 Python 绑定是编译过的，公开接口里没有一个明确叫
|
||||
"force vector map" 的方法。按语义最接近的是 ``compute_flow()``
|
||||
（形变光流 ≈ 切向位移场 ≈ 力矢量场），其次是 ``compute_hhd()``。
|
||||
不同 SDK 版本这两个方法的返回值也不一样 —— 有的是 ``(H,W,2)`` 数组，
|
||||
有的返回 ``bool``（只给"形变是否超阈值"）或可视化图。
|
||||
|
||||
所以这里逐个试，只接受真正是二维以上数组的结果；都拿不到就返回 ``None``，
|
||||
上层跳过该话题而不是发一张假图。哪一路成功会打到日志里，便于现场核对。
|
||||
"""
|
||||
for name in ("compute_flow", "compute_hhd", "compute_deformation"):
|
||||
fn = getattr(sensor, name, None)
|
||||
if fn is None:
|
||||
continue
|
||||
try:
|
||||
result = fn()
|
||||
except Exception:
|
||||
continue
|
||||
if isinstance(result, np.ndarray) and result.ndim >= 2 and result.size:
|
||||
return result
|
||||
return None
|
||||
|
||||
|
||||
def _parse_video(value) -> int | str:
|
||||
if isinstance(value, int):
|
||||
return value
|
||||
text = str(value).strip()
|
||||
if text.isdigit():
|
||||
return int(text)
|
||||
return text
|
||||
|
||||
|
||||
class OrisysTactile(TactileInterface):
|
||||
name = "orisys_tactile"
|
||||
|
||||
def __init__(self, config: OrisysDualConfig | None = None) -> None:
|
||||
self.cfg = config or OrisysDualConfig(
|
||||
left=OrisysSensorConfig(vid_src=0),
|
||||
right=OrisysSensorConfig(vid_src=2),
|
||||
)
|
||||
self._sensors: list[dict[str, Any]] = []
|
||||
self._baseline: dict[str, float] = {}
|
||||
self._connected = False
|
||||
|
||||
def connect(self, sides=("left", "right")) -> None:
|
||||
"""连接指定侧的传感器。注意：SDK 单进程双开会严重掉帧（30→0.8fps），
|
||||
双指请拆两个进程各连一侧（tactile.launch.py 已是双进程）。"""
|
||||
try:
|
||||
import orisys
|
||||
except ImportError as exc:
|
||||
raise RuntimeError(
|
||||
"缺少 orisys SDK。请执行 pip install -e /path/to/orisys-sdk "
|
||||
"（git.orisys-tech.cn/orisys/test）。配置依旧从 ``site.yaml`` 的 ``tactile`` 段读取。"
|
||||
) from exc
|
||||
|
||||
self._sensors = []
|
||||
for side, cfg in (("left", self.cfg.left), ("right", self.cfg.right)):
|
||||
if side not in sides:
|
||||
continue
|
||||
kwargs = dict(
|
||||
cuda=cfg.cuda,
|
||||
isstitch=cfg.isstitch,
|
||||
verbose=False,
|
||||
config_name=cfg.config_name,
|
||||
backend=cfg.backend,
|
||||
)
|
||||
if cfg.rotation_config_path:
|
||||
kwargs["rotation_config_path"] = cfg.rotation_config_path
|
||||
sensor = orisys.Sensor(cfg.vid_src, **kwargs)
|
||||
self._sensors.append({"side": side, "sensor": sensor, "cfg": cfg})
|
||||
self._calibrate_baseline()
|
||||
self._connected = True
|
||||
|
||||
def read_side(self, side: str, want_vector: bool = False) -> dict[str, Any]:
|
||||
"""读指定侧（单侧进程模式）；未连接/未知侧返回全零。
|
||||
|
||||
``want_vector=True`` 时额外计算力矢量场（逐像素运算，较贵）——
|
||||
只应在需要发布矢量图的那些帧上打开。
|
||||
"""
|
||||
if not self._connected:
|
||||
return self._empty_sample(side)
|
||||
for item in self._sensors:
|
||||
if item["side"] == side:
|
||||
return self._read_single(item, want_vector=want_vector)
|
||||
return self._empty_sample(side)
|
||||
|
||||
def disconnect(self) -> None:
|
||||
for item in self._sensors:
|
||||
try:
|
||||
item["sensor"].disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
self._sensors = []
|
||||
self._connected = False
|
||||
|
||||
def reset(self, hole_id: int) -> None:
|
||||
del hole_id
|
||||
|
||||
def step(self, sample: TactileSample) -> TactileCommand:
|
||||
del sample
|
||||
return TactileCommand(
|
||||
delta_xyz=np.zeros(3),
|
||||
delta_rpy=np.zeros(3),
|
||||
done=False,
|
||||
ok=True,
|
||||
message="orisys step",
|
||||
)
|
||||
|
||||
def read_sample(self) -> TactileSample:
|
||||
if not self._connected or len(self._sensors) != 2:
|
||||
return TactileSample(
|
||||
timestamp=time.time(),
|
||||
force_xyz=np.zeros(3),
|
||||
torque_xyz=np.zeros(3),
|
||||
contact=False,
|
||||
)
|
||||
left = self._read_single(self._sensors[0])
|
||||
right = self._read_single(self._sensors[1])
|
||||
force_xyz = np.array(
|
||||
[
|
||||
(left["fnormal"] + right["fnormal"]) * 0.5,
|
||||
(left["fshearx"] + right["fshearx"]) * 0.5,
|
||||
(left["fsheary"] + right["fsheary"]) * 0.5,
|
||||
]
|
||||
)
|
||||
torque_xyz = np.array(
|
||||
[
|
||||
right["fnormal"] - left["fnormal"],
|
||||
right["fshearx"] - left["fshearx"],
|
||||
right["fsheary"] - left["fsheary"],
|
||||
]
|
||||
)
|
||||
return TactileSample(
|
||||
timestamp=time.time(),
|
||||
force_xyz=force_xyz,
|
||||
torque_xyz=torque_xyz,
|
||||
contact=bool(left["is_contact"] or right["is_contact"]),
|
||||
embedding=np.array(
|
||||
[
|
||||
left["fnormal"],
|
||||
right["fnormal"],
|
||||
left["fshearx"],
|
||||
right["fshearx"],
|
||||
left["fsheary"],
|
||||
right["fsheary"],
|
||||
]
|
||||
),
|
||||
raw={"left": left, "right": right, "centroid": self._estimate_centroid(left, right)},
|
||||
)
|
||||
|
||||
def _read_single(self, item: dict, want_vector: bool = False) -> dict[str, Any]:
|
||||
sensor = item["sensor"]
|
||||
cfg = item["cfg"]
|
||||
img = sensor.get_img()
|
||||
if img is None:
|
||||
return self._empty_sample(item["side"])
|
||||
is_contact = sensor.compute_deformation(check_motion=True, threshold=cfg.motion_threshold)
|
||||
fps, fnormal, fshearx, fsheary = sensor.read_info(
|
||||
sensor.info.FPS,
|
||||
sensor.info.FNORMAL,
|
||||
sensor.info.FSHEARX,
|
||||
sensor.info.FSHEARY,
|
||||
)
|
||||
side = item["side"]
|
||||
fnormal = max(0.0, float(fnormal) - self._baseline.get(f"{side}_normal", 0.0))
|
||||
fshearx = float(fshearx) - self._baseline.get(f"{side}_shearx", 0.0)
|
||||
fsheary = float(fsheary) - self._baseline.get(f"{side}_sheary", 0.0)
|
||||
centroid = (0.0, 0.0)
|
||||
centroid_found = False
|
||||
try:
|
||||
sensor.compute_contact()
|
||||
c, found = sensor.read_info(sensor.info.CENTROID)
|
||||
if found:
|
||||
centroid = c
|
||||
centroid_found = True
|
||||
except Exception:
|
||||
pass
|
||||
return {
|
||||
"side": side,
|
||||
"fnormal": fnormal,
|
||||
"fshearx": fshearx,
|
||||
"fsheary": fsheary,
|
||||
"fps": float(fps),
|
||||
"is_contact": bool(is_contact),
|
||||
"centroid": centroid,
|
||||
"centroid_found": centroid_found,
|
||||
"img": img,
|
||||
# 矢量场只在被要求时才算：compute_flow 是逐像素运算，每帧都做会
|
||||
# 明显增加 worker 耗时，可能拖累三力的 30fps。
|
||||
"vector": read_vector_map(sensor) if want_vector else None,
|
||||
}
|
||||
|
||||
def _empty_sample(self, side: str = "unknown") -> dict[str, Any]:
|
||||
return {
|
||||
"side": side,
|
||||
"fnormal": 0.0,
|
||||
"fshearx": 0.0,
|
||||
"fsheary": 0.0,
|
||||
"fps": 0.0,
|
||||
"is_contact": False,
|
||||
"centroid": (0.0, 0.0),
|
||||
"centroid_found": False,
|
||||
"img": None,
|
||||
"vector": None,
|
||||
}
|
||||
|
||||
def _calibrate_baseline(self) -> None:
|
||||
seconds = self.cfg.baseline_seconds
|
||||
if seconds <= 0:
|
||||
return
|
||||
buckets: dict[str, list[float]] = {
|
||||
"left_normal": [],
|
||||
"right_normal": [],
|
||||
"left_shearx": [],
|
||||
"right_shearx": [],
|
||||
"left_sheary": [],
|
||||
"right_sheary": [],
|
||||
}
|
||||
deadline = time.perf_counter() + seconds
|
||||
while time.perf_counter() < deadline:
|
||||
for item in self._sensors:
|
||||
sensor = item["sensor"]
|
||||
side = item["side"]
|
||||
img = sensor.get_img()
|
||||
if img is None:
|
||||
continue
|
||||
sensor.compute_deformation(check_motion=False)
|
||||
fnormal, fshearx, fsheary = sensor.read_info(
|
||||
sensor.info.FNORMAL,
|
||||
sensor.info.FSHEARX,
|
||||
sensor.info.FSHEARY,
|
||||
)
|
||||
buckets[f"{side}_normal"].append(float(fnormal))
|
||||
buckets[f"{side}_shearx"].append(float(fshearx))
|
||||
buckets[f"{side}_sheary"].append(float(fsheary))
|
||||
time.sleep(0.01)
|
||||
self._baseline = {k: float(np.mean(v)) if v else 0.0 for k, v in buckets.items()}
|
||||
|
||||
def _estimate_centroid(self, left: dict, right: dict) -> tuple[float, float]:
|
||||
if left["centroid_found"] and right["centroid_found"]:
|
||||
return (
|
||||
(left["centroid"][0] + right["centroid"][0]) * 0.5,
|
||||
(left["centroid"][1] + right["centroid"][1]) * 0.5,
|
||||
)
|
||||
if left["centroid_found"]:
|
||||
return left["centroid"]
|
||||
if right["centroid_found"]:
|
||||
return right["centroid"]
|
||||
return (0.0, 0.0)
|
||||
|
||||
|
||||
class MockOrisysTactile(OrisysTactile):
|
||||
name = "mock_orisys"
|
||||
|
||||
def __init__(self, config: OrisysDualConfig | None = None) -> None:
|
||||
super().__init__(config)
|
||||
self._n = 0
|
||||
|
||||
def connect(self) -> None:
|
||||
self._connected = True
|
||||
self._baseline = {}
|
||||
self._n = 0
|
||||
self._sensors = [
|
||||
{"side": "left", "sensor": None, "cfg": self.cfg.left},
|
||||
{"side": "right", "sensor": None, "cfg": self.cfg.right},
|
||||
]
|
||||
|
||||
def read_sample(self) -> TactileSample:
|
||||
self._n += 1
|
||||
contact = self._n >= 8
|
||||
force = 0.2 if contact else 0.0
|
||||
return TactileSample(
|
||||
timestamp=time.time(),
|
||||
force_xyz=np.array([force, 0.01, 0.005]),
|
||||
torque_xyz=np.zeros(3),
|
||||
contact=contact,
|
||||
raw={
|
||||
"left": {"fnormal": force, "fshearx": 0.0, "fsheary": 0.0, "is_contact": contact, "fps": 30.0, "centroid": (0, 0), "centroid_found": False},
|
||||
"right": {"fnormal": force, "fshearx": 0.0, "fsheary": 0.0, "is_contact": contact, "fps": 30.0, "centroid": (0, 0), "centroid_found": False},
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def make_tactile(data: dict) -> TactileInterface:
|
||||
cfg = load_orisys_from_site(data)
|
||||
backend = (cfg.backend or "orisys").strip().lower()
|
||||
if backend in ("mock", "mock_orisys"):
|
||||
return MockOrisysTactile(cfg)
|
||||
return OrisysTactile(cfg)
|
||||
|
||||
|
||||
# ==================== 多进程工作器（厂家推荐架构，每进程独立 CUDA 上下文） ====================
|
||||
# 实测：单进程双开两路传感器会互相拖垮（30→0.8fps）；两个独立进程各开一路则稳定 30fps。
|
||||
|
||||
def _usb_devpath_for_video(vid_src) -> Optional[str]:
|
||||
"""/dev/v4l/by-path/... 或 /dev/videoN → sysfs USB 设备号（如 1-3.4.1）。"""
|
||||
import re
|
||||
|
||||
try:
|
||||
real = os.path.realpath(str(vid_src))
|
||||
base = os.path.basename(real)
|
||||
if not base.startswith("video"):
|
||||
return None
|
||||
sysdev = os.path.realpath(f"/sys/class/video4linux/{base}/device")
|
||||
for part in reversed(sysdev.split("/")):
|
||||
if re.fullmatch(r"\d+-\d+(\.\d+)*", part):
|
||||
return part
|
||||
except Exception:
|
||||
pass
|
||||
return None
|
||||
|
||||
|
||||
def _usb_self_heal(vid_src, side: str) -> None:
|
||||
"""内核级卡死（select 超时循环）只有 unbind/bind 能救。orisys-usb-reset
|
||||
是 root 拥有的白名单脚本（sudoers NOPASSWD），只认 Orisys 的 vid/pid。"""
|
||||
import subprocess
|
||||
|
||||
dev = _usb_devpath_for_video(vid_src)
|
||||
if not dev:
|
||||
return f"[{side}] USB 自愈：解析不到设备号（{vid_src}）"
|
||||
try:
|
||||
r = subprocess.run(
|
||||
["sudo", "-n", "/usr/local/bin/orisys-usb-reset", dev],
|
||||
capture_output=True, text=True, timeout=15.0,
|
||||
)
|
||||
note = (r.stdout or r.stderr).strip().splitlines()
|
||||
return f"[{side}] USB 自愈复位 {dev}: {note[-1] if note else 'ok'}"
|
||||
except Exception as exc:
|
||||
return f"[{side}] USB 自愈失败 {dev}: {exc}"
|
||||
|
||||
|
||||
def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event, image_q=None) -> None:
|
||||
"""独立进程：开一侧传感器，把样本推进队列。
|
||||
|
||||
q: mp.Queue（调用方给 maxsize≈2，满了丢旧留新）—— 三力等轻量数据，每帧都推。
|
||||
image_q: mp.Queue 或 None —— 视触觉图像与力矢量场。
|
||||
|
||||
图像默认**不过队列**（``image_q=None``）：每帧几百 KB，跨进程序列化 +
|
||||
ROS 发布的开销会挤占三力的 30fps 时序。需要图像时调用方传一个
|
||||
``maxsize=1`` 的队列，并按 ``cfg.image_hz`` 节流推送 —— 队列满时丢旧留新，
|
||||
主进程来不及消费也只会丢图像，不会影响三力。
|
||||
|
||||
stop_event: mp.Event，置位后干净退出（先 disconnect 释放 V4L2 设备）
|
||||
|
||||
自动重连：摄像头被强占/USB 抖动后会出现 get_img 持续 select 超时（空读，
|
||||
单次最长阻塞 ~10s）。连续 2 次空读或单次读取 >2s 即断开重连，直到恢复。
|
||||
"""
|
||||
try: # 给进程起个可识别的 comm 名，便于 pkill 清理孤儿（否则占死 /dev/video*）
|
||||
import ctypes
|
||||
ctypes.CDLL("libc.so.6").prctl(15, b"orisys_tac_w", 0, 0, 0) # PR_SET_NAME
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _put(item: dict) -> None:
|
||||
try:
|
||||
if q.full():
|
||||
try:
|
||||
q.get_nowait()
|
||||
except Exception:
|
||||
pass
|
||||
q.put(item)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _put_image(item: dict) -> None:
|
||||
"""图像入队：只保留最新一帧。队列满说明主进程没在消费，丢掉旧的即可。"""
|
||||
try:
|
||||
if image_q is None:
|
||||
return
|
||||
if image_q.full():
|
||||
try:
|
||||
image_q.get_nowait()
|
||||
except Exception:
|
||||
pass
|
||||
image_q.put(item)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
period = 1.0 / max(1.0, float(getattr(cfg.left if side == "left" else cfg.right, "target_fps", 30.0)))
|
||||
img_period = (1.0 / cfg.image_hz) if (image_q is not None and cfg.image_hz > 0) else 0.0
|
||||
last_img_t = 0.0
|
||||
vid_src = getattr(cfg.left if side == "left" else cfg.right, "vid_src", None)
|
||||
tac: Optional[OrisysTactile] = None
|
||||
bad = 0
|
||||
dead_cycles = 0 # 连续“重连了但读不到数据”的轮数 → 内核级卡死 → USB 自愈
|
||||
backoff = 0.5
|
||||
while not stop_event.is_set():
|
||||
if tac is None:
|
||||
try:
|
||||
tac = OrisysTactile(cfg)
|
||||
tac.connect(sides=(side,))
|
||||
_put({"side": side, "error": f"[{side}] (re)connected"})
|
||||
backoff = 0.5
|
||||
except Exception as exc:
|
||||
_put({"side": side, "error": f"connect failed: {exc}"})
|
||||
tac = None
|
||||
dead_cycles += 1
|
||||
if dead_cycles >= 2:
|
||||
_put({"side": side, "error": _usb_self_heal(vid_src, side)})
|
||||
dead_cycles = 0
|
||||
time.sleep(min(backoff, 3.0))
|
||||
backoff *= 2.0
|
||||
continue
|
||||
# 只有图像帧才算矢量场：compute_flow 是逐像素运算，每帧都算会拖累三力。
|
||||
want_img = image_q is not None and (time.perf_counter() - last_img_t) >= img_period
|
||||
t0 = time.perf_counter()
|
||||
try:
|
||||
d = tac.read_side(side, want_vector=want_img)
|
||||
empty = d.get("img") is None
|
||||
except Exception:
|
||||
d, empty = None, True
|
||||
dt = time.perf_counter() - t0
|
||||
if empty or dt > 2.0:
|
||||
bad += 1
|
||||
if bad >= 2:
|
||||
_put({"side": side, "error": f"[{side}] 相机断流，自动重连中…"})
|
||||
try:
|
||||
tac.disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
tac = None
|
||||
bad = 0
|
||||
dead_cycles += 1
|
||||
if dead_cycles >= 2:
|
||||
_put({"side": side, "error": _usb_self_heal(vid_src, side)})
|
||||
dead_cycles = 0
|
||||
continue
|
||||
else:
|
||||
bad = 0
|
||||
dead_cycles = 0
|
||||
if d is not None and not empty:
|
||||
if want_img:
|
||||
last_img_t = time.perf_counter()
|
||||
_put_image({
|
||||
"side": side,
|
||||
"img": d.get("img"),
|
||||
"vector": d.get("vector"),
|
||||
"stamp": time.time(),
|
||||
})
|
||||
# 图像与矢量场已单独走 image_q；三力这条轻量队列不背它们。
|
||||
d = {k: v for k, v in d.items() if k not in ("img", "vector")}
|
||||
d["error"] = ""
|
||||
_put(d)
|
||||
rest = period - (time.perf_counter() - t0)
|
||||
if rest > 0:
|
||||
time.sleep(rest)
|
||||
try:
|
||||
if tac is not None:
|
||||
tac.disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
@@ -0,0 +1,820 @@
|
||||
"""D405 主跟踪丢了之后，用顶置 D435 **拍一张** 把工件 / k0 定到基座系。
|
||||
|
||||
为什么单独成模块
|
||||
----------------
|
||||
一阶段平时走腕部 D405（眼在手上）。画面里没有 conf≥门槛的目标时，
|
||||
不能让伺服继续追 0.12 的假框，也不能把顶视外参再乘一遍法兰
|
||||
（那会让一次性定位差几十厘米还看起来合理）。
|
||||
|
||||
判定、取 k0、反投影、臂是否静止 —— 全是纯函数，测试不启 ROS。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Iterable, Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.handeye_lib import cam_point_to_base
|
||||
from arm_application.path_profile import ( # 伺服和测试从本模块再导出，不另抄一份
|
||||
D435_FULL_ACC_M,
|
||||
D435_FULL_CRUISE_MPS,
|
||||
D435_FULL_DEC_M,
|
||||
D435_MID_HOP_MAX_M,
|
||||
D435_MID_LEFTOVER_M,
|
||||
D435_MOVE_ACC_MPS2,
|
||||
D435_MOVE_SPEED_SCALE,
|
||||
D435_NEAR_CRUISE_MPS,
|
||||
D435_NEAR_HOP_MAX_M,
|
||||
D435_NEAR_LEFTOVER_M,
|
||||
D435_VENDOR_MOVEL_ACC_MPS2,
|
||||
D435_VENDOR_MOVEL_VEL_MPS,
|
||||
D435_VERT_TILT_RAD,
|
||||
D435_SETTLE_S,
|
||||
FROZEN_J_HOP_MAX_M,
|
||||
build_arc_xy_move,
|
||||
flange_down_w,
|
||||
flange_tilt_rad,
|
||||
hybrid_cruise_vxy,
|
||||
oneshot_hop_limits,
|
||||
smooth_joint_schedule,
|
||||
trap_acc_for_hop,
|
||||
translate_hold_ori_waypoints,
|
||||
)
|
||||
|
||||
# 上一跳实际少走这么多，就认定冻结 J 在当前姿态走不动，不许按同一剩差再刷。
|
||||
D435_MIN_PROGRESS_FRAC = 0.40
|
||||
|
||||
|
||||
def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
|
||||
"""画面内最高检测置信度。空列表 / None → ``None``（不是 0：0 是"看到了但很差"）。"""
|
||||
if not dets:
|
||||
return None
|
||||
vals = [float(getattr(d, "conf", float("nan"))) for d in dets]
|
||||
vals = [c for c in vals if np.isfinite(c)]
|
||||
if not vals:
|
||||
return None
|
||||
return max(vals)
|
||||
|
||||
|
||||
def z_locked_qos():
|
||||
"""``visual_servo/z_locked`` 的 QoS。发布端和订阅端必须用这一份。
|
||||
|
||||
感知比伺服晚启动。默认 volatile 会丢掉伺服启动时已经发出的「已到锁高」，
|
||||
感知就停在初值 False，D435 永远不拍。
|
||||
"""
|
||||
from rclpy.qos import ( # noqa: PLC0415 — 纯函数测试不需要在导入时拉起 ROS
|
||||
DurabilityPolicy,
|
||||
HistoryPolicy,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
)
|
||||
|
||||
return QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
)
|
||||
|
||||
|
||||
def at_lock_height(z_m: float, lock_z_m: float, gate_m: float) -> bool:
|
||||
"""法兰是否在锁高带内。
|
||||
|
||||
低于锁高**不算**到位。上一轮观测下降停在 0.26 m 时，若把「不高于锁高」
|
||||
当成到位，XY 会在低位开跑，D435 又因高度不对拒绝拍照，两边都不再动。
|
||||
``lock_z_m <= 0`` 表示没启用锁高。
|
||||
"""
|
||||
lock = float(lock_z_m)
|
||||
if lock <= 0.0:
|
||||
return True
|
||||
gate = max(0.02, float(gate_m))
|
||||
return abs(float(z_m) - lock) <= gate
|
||||
|
||||
|
||||
def lock_z_velocity(z_m: float, lock_z_m: float, *, kp: float, v_lim: float) -> float:
|
||||
"""回到锁高的竖直速度 [m/s]，基座 +Z 朝上。
|
||||
|
||||
低于锁高为正（上抬），高于为负（下降）。4 mm 内视为到位。
|
||||
不许在低于锁高时把速度写成 0：那会和「没到锁高不拍 D435」卡死。
|
||||
"""
|
||||
lock = float(lock_z_m)
|
||||
if lock <= 0.0:
|
||||
return 0.0
|
||||
ez = lock - float(z_m)
|
||||
if abs(ez) < 0.004:
|
||||
return 0.0
|
||||
lim = abs(float(v_lim))
|
||||
return float(np.clip(float(kp) * ez, -lim, lim))
|
||||
|
||||
|
||||
def hold_translation_vz(z_m: float, z_hold_m: float, *, kp: float, v_lim: float) -> float:
|
||||
"""平移时把法兰钉在**这一段开始时的高度**。
|
||||
|
||||
只写 v_z=0 不够：大范围平移加姿态校正时，阻尼求逆会把竖直速度吃掉，
|
||||
2026-10-09 真机从 0.327 m 漂到 0.307 m 又抬到 0.337 m。这里用开始那一
|
||||
帧的高度当设定，4 mm 内不动，避免来回抖。不追 site 里的锁高数字，
|
||||
也不因此停掉 XY。下臂仍只在二阶段。
|
||||
"""
|
||||
ez = float(z_hold_m) - float(z_m)
|
||||
if abs(ez) < 0.004:
|
||||
return 0.0
|
||||
lim = abs(float(v_lim))
|
||||
return float(np.clip(float(kp) * ez, -lim, lim))
|
||||
|
||||
|
||||
# D405 连续这么久没有高置信度目标，才让待命的 D435 拍一张。
|
||||
# 单帧漏检就拍，会在平移半路把目标换掉。
|
||||
D405_MISS_BEFORE_D435_S = 0.5
|
||||
# 拍了但伺服没 ``d435_commanding``（工作盒拒收等）：超过这时限就允许再拍。
|
||||
# 不清的话 ``_oneshot_shot`` 会永远真，臂停着、感知还以为在赶路。
|
||||
ONESHOT_ACCEPT_TIMEOUT_S = 0.8
|
||||
|
||||
|
||||
def oneshot_shot_timed_out(
|
||||
*,
|
||||
shot: bool,
|
||||
commanding: bool,
|
||||
age_s: float,
|
||||
timeout_s: float = ONESHOT_ACCEPT_TIMEOUT_S,
|
||||
) -> bool:
|
||||
"""拍过一张、伺服却没接住 → True。已经在赶路不许清。"""
|
||||
if not bool(shot) or bool(commanding):
|
||||
return False
|
||||
return float(age_s) >= float(timeout_s)
|
||||
|
||||
|
||||
def d405_commands_arm(
|
||||
*,
|
||||
using_d435: bool,
|
||||
arrived: bool,
|
||||
settled: bool,
|
||||
move_only: bool = False,
|
||||
) -> bool:
|
||||
"""D405 这一帧的点能不能下发给左臂。
|
||||
|
||||
推理一直开着。正在用 D435 那一个点时，进程内 MoveL 结束并且停稳之前不下发。
|
||||
停稳用 ``d435_handback_sec`` 和 ``lock_still_move_m``。赶路时 XY 已经进 15 mm
|
||||
也不算到位。不许在 MoveL 期间下发 D405（两套控制互抢会晃）。
|
||||
``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
|
||||
"""
|
||||
if move_only:
|
||||
return False
|
||||
if not using_d435:
|
||||
return True
|
||||
return bool(arrived) and bool(settled)
|
||||
|
||||
|
||||
def oneshot_needs_verticalize(
|
||||
tilt_rad: float,
|
||||
leftover_m: float,
|
||||
arrive_m: float,
|
||||
*,
|
||||
tilt_lim: float = D435_VERT_TILT_RAD,
|
||||
already_done: bool = False,
|
||||
) -> bool:
|
||||
"""XY 已经进到位门槛、倾角还大，才单独收垂直。不许和平移捆在一起。"""
|
||||
if already_done:
|
||||
return False
|
||||
if float(leftover_m) > float(arrive_m):
|
||||
return False
|
||||
return float(tilt_rad) > float(tilt_lim)
|
||||
|
||||
|
||||
def oneshot_next_hop_m(
|
||||
dist_m: float, hop_max_m: float = FROZEN_J_HOP_MAX_M
|
||||
) -> float:
|
||||
"""这一跳最多走 ``hop_max_m``。冻结 J 一次规划 400 mm 会少走一大截。"""
|
||||
return min(max(0.0, float(dist_m)), float(hop_max_m))
|
||||
|
||||
|
||||
def oneshot_cartesian_dp(
|
||||
p_ee,
|
||||
goal_xy,
|
||||
lock_z: float,
|
||||
*,
|
||||
gate_m: float,
|
||||
hop_max_m: Optional[float] = None,
|
||||
leftover_xy_m: Optional[float] = None,
|
||||
):
|
||||
"""这一跳的笛卡尔位移。没到锁高只收 Z；远处 XY 带锁高；近处只走 XY。
|
||||
|
||||
工件上方最后一跳若再把 7 mm 的 Z 和 60 mm 的 XY 捆在一起，冻结 J 会把腕部带偏。
|
||||
远处仍带 ``ee_z_m``，避免长跳把高度抬出锁高带。
|
||||
"""
|
||||
p = np.asarray(p_ee, dtype=float).reshape(3)
|
||||
z_lock = float(lock_z)
|
||||
leftover = (
|
||||
float(leftover_xy_m)
|
||||
if leftover_xy_m is not None
|
||||
else float(
|
||||
np.hypot(float(goal_xy[0]) - float(p[0]), float(goal_xy[1]) - float(p[1]))
|
||||
)
|
||||
)
|
||||
hop_cap = (
|
||||
float(hop_max_m)
|
||||
if hop_max_m is not None
|
||||
else oneshot_hop_limits(leftover)[0]
|
||||
)
|
||||
if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
|
||||
dz = z_lock - float(p[2])
|
||||
mag = oneshot_next_hop_m(abs(dz), hop_cap)
|
||||
if mag < 1e-6:
|
||||
return np.zeros(3, dtype=float)
|
||||
return np.array([0.0, 0.0, float(np.sign(dz)) * mag], dtype=float)
|
||||
dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
|
||||
dp = np.array(
|
||||
[
|
||||
float(goal_xy[0]) - float(p[0]),
|
||||
float(goal_xy[1]) - float(p[1]),
|
||||
dz,
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
n = float(np.linalg.norm(dp))
|
||||
hop = oneshot_next_hop_m(n, hop_cap)
|
||||
if n > hop > 1e-9:
|
||||
dp = dp * (hop / n)
|
||||
return dp
|
||||
|
||||
|
||||
def oneshot_full_segments(
|
||||
p_ee,
|
||||
goal_xy,
|
||||
lock_z: float,
|
||||
*,
|
||||
gate_m: float,
|
||||
):
|
||||
"""整段一次走完的笛卡尔分段。先收齐锁高，再走全部 XY，不切 80/15 mm。
|
||||
|
||||
近处（剩差 ≤80 mm）仍然只走 XY，不混 Z。收垂直不在这里。
|
||||
返回 ``[(dp, tag), ...]``，``tag`` 是 ``lift_z`` 或 ``xy``。
|
||||
"""
|
||||
p = np.asarray(p_ee, dtype=float).reshape(3).copy()
|
||||
z_lock = float(lock_z)
|
||||
goal = np.asarray(goal_xy, dtype=float).reshape(2)
|
||||
out = []
|
||||
if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
|
||||
dz = z_lock - float(p[2])
|
||||
if abs(dz) >= 1e-6:
|
||||
out.append((np.array([0.0, 0.0, dz], dtype=float), "lift_z"))
|
||||
p[2] = z_lock
|
||||
leftover = float(np.hypot(float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1])))
|
||||
dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
|
||||
dp = np.array(
|
||||
[float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1]), dz],
|
||||
dtype=float,
|
||||
)
|
||||
if float(np.linalg.norm(dp)) >= 1e-6:
|
||||
out.append((dp, "xy"))
|
||||
return out
|
||||
|
||||
|
||||
def oneshot_concat_hold_ori(q0, J, segments, *, R=None):
|
||||
"""把分段 ``dp`` 用同一份冻结 J 串成一条关节折线。中途不回零速。"""
|
||||
q = np.asarray(q0, dtype=float).copy()
|
||||
wps = [q.copy()]
|
||||
planned = 0.0
|
||||
asked = 0.0
|
||||
tags = []
|
||||
for dp, tag in segments:
|
||||
hop = float(np.linalg.norm(np.asarray(dp, dtype=float).reshape(3)))
|
||||
if hop < 1e-6:
|
||||
continue
|
||||
asked += hop
|
||||
part, remain = translate_hold_ori_waypoints(
|
||||
q, J, dp, R=R, verticalize=False
|
||||
)
|
||||
if len(part) < 2:
|
||||
break
|
||||
wps.extend(part[1:])
|
||||
q = wps[-1]
|
||||
planned += hop - float(remain)
|
||||
tags.append(str(tag))
|
||||
tag = "full"
|
||||
if tags == ["lift_z"]:
|
||||
tag = "lift_z"
|
||||
elif "lift_z" in tags and "xy" in tags:
|
||||
tag = "full"
|
||||
elif tags == ["xy"]:
|
||||
tag = "full"
|
||||
return wps, planned, asked, tag
|
||||
|
||||
|
||||
def oneshot_full_cruise_mps(leftover_m: float, far_cruise_mps: float) -> float:
|
||||
"""整段巡航：工件上方 45 mm/s；远处封顶 ``D435_FULL_CRUISE_MPS``，不许再弹到远处 hop 上限。"""
|
||||
far = max(1e-4, float(far_cruise_mps))
|
||||
if float(leftover_m) <= D435_NEAR_LEFTOVER_M:
|
||||
return min(D435_NEAR_CRUISE_MPS, far)
|
||||
return min(D435_FULL_CRUISE_MPS, far)
|
||||
|
||||
|
||||
def oneshot_may_send_another(
|
||||
leftover_m: float,
|
||||
last_dist_m: Optional[float],
|
||||
*,
|
||||
arrive_m: float,
|
||||
hop_max_m: float = FROZEN_J_HOP_MAX_M,
|
||||
min_progress_frac: float = D435_MIN_PROGRESS_FRAC,
|
||||
) -> bool:
|
||||
"""上一跳走完后还要不要再发一条。
|
||||
|
||||
剩差已经进到位门槛 → 钉住。
|
||||
上一跳几乎没缩短剩差 → 不许按同一距离再刷（真机 28 mm 死循环）。
|
||||
"""
|
||||
if float(leftover_m) <= float(arrive_m):
|
||||
return False
|
||||
if last_dist_m is None:
|
||||
return True
|
||||
progressed = float(last_dist_m) - float(leftover_m)
|
||||
planned = min(float(last_dist_m), float(hop_max_m))
|
||||
if progressed < float(min_progress_frac) * max(planned, 1e-6):
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def oneshot_should_submit(
|
||||
*,
|
||||
handoff: bool,
|
||||
low_for_s: float,
|
||||
already_shot: bool,
|
||||
wait_s: float = D405_MISS_BEFORE_D435_S,
|
||||
) -> bool:
|
||||
"""腕部连续 ``wait_s`` 秒没有高置信度目标、这一轮还没拍成，才让 D435 拍一张。
|
||||
|
||||
高度不在这里再判一次。进一阶段之前已经抬到锁高；再等伺服报「已到锁高」
|
||||
会和「没到锁高就只收 Z、不平移」卡死。拍成一张之后不许连拍，顶视不是第二套跟踪。
|
||||
"""
|
||||
return (
|
||||
bool(handoff)
|
||||
and (not already_shot)
|
||||
and float(low_for_s) >= float(wait_s)
|
||||
)
|
||||
|
||||
|
||||
def select_oneshot_det(dets: Optional[Iterable], min_conf: float):
|
||||
"""顶视一次性：只收 conf≥门槛的框，取最高分。没有就 None，不许降级。"""
|
||||
if not dets:
|
||||
return None
|
||||
ok = [
|
||||
d
|
||||
for d in dets
|
||||
if np.isfinite(float(getattr(d, "conf", float("nan"))))
|
||||
and float(d.conf) >= float(min_conf)
|
||||
]
|
||||
if not ok:
|
||||
return None
|
||||
return max(ok, key=lambda d: float(d.conf))
|
||||
|
||||
|
||||
def depth_raw_to_m(raw: Optional[np.ndarray], encoding: str) -> Optional[np.ndarray]:
|
||||
"""深度图 → 米。16UC1 常见是毫米；用中位数判量纲，不许默认当米。"""
|
||||
if raw is None:
|
||||
return None
|
||||
enc = (encoding or "").lower()
|
||||
arr = np.asarray(raw)
|
||||
if arr.size == 0:
|
||||
return None
|
||||
if enc in ("16uc1", "mono16"):
|
||||
pos = arr[arr > 0]
|
||||
if pos.size < 8:
|
||||
return None
|
||||
med = float(np.median(pos))
|
||||
scale = 0.001 if 20.0 <= med <= 8000.0 else (1.0 if med < 20 else 0.0001)
|
||||
out = arr.astype(float) * scale
|
||||
out[arr <= 0] = np.nan
|
||||
return out
|
||||
if enc == "32fc1":
|
||||
return arr.astype(float)
|
||||
return None
|
||||
|
||||
|
||||
def map_uv_to_depth(
|
||||
u: float, v: float, color_wh: tuple[int, int], depth_wh: tuple[int, int]
|
||||
) -> tuple[float, float]:
|
||||
"""彩色图像素 → 对齐深度图像素。分辨率相同则原样返回。"""
|
||||
cw, ch = int(color_wh[0]), int(color_wh[1])
|
||||
dw, dh = int(depth_wh[0]), int(depth_wh[1])
|
||||
if cw <= 0 or ch <= 0 or (cw == dw and ch == dh):
|
||||
return float(u), float(v)
|
||||
return float(u) * (dw / float(cw)), float(v) * (dh / float(ch))
|
||||
|
||||
|
||||
def wrist_needs_handoff(best_conf: Optional[float], threshold: float) -> bool:
|
||||
"""腕部这一帧有没有 conf≥门槛的目标。没有 → 该切 D435。
|
||||
|
||||
``None`` 当没有检测。门槛必须显式传入，不许在这里写死再抄一份到 site.yaml。
|
||||
"""
|
||||
if best_conf is None:
|
||||
return True
|
||||
try:
|
||||
c = float(best_conf)
|
||||
except (TypeError, ValueError):
|
||||
return True
|
||||
if not np.isfinite(c):
|
||||
return True
|
||||
return c < float(threshold)
|
||||
|
||||
|
||||
def k0_uv_from_det(
|
||||
det,
|
||||
*,
|
||||
kpt_index: int = 0,
|
||||
kpt_min_conf: float = 0.30,
|
||||
) -> Optional[tuple[float, float]]:
|
||||
"""从 pose 检测取 k0 像素。k0 不可信就返回 None（不许拿框心冒充 k0）。"""
|
||||
kpts = getattr(det, "kpts", None)
|
||||
if kpts is None:
|
||||
return None
|
||||
arr = np.asarray(kpts, dtype=float)
|
||||
if arr.ndim != 2 or arr.shape[0] <= int(kpt_index) or arr.shape[1] < 3:
|
||||
return None
|
||||
u, v, kc = (float(x) for x in arr[int(kpt_index)][:3])
|
||||
if not (np.isfinite(u) and np.isfinite(v) and np.isfinite(kc)):
|
||||
return None
|
||||
if kc < float(kpt_min_conf):
|
||||
return None
|
||||
return u, v
|
||||
|
||||
|
||||
def workpiece_uv_from_det(
|
||||
det,
|
||||
*,
|
||||
kpt_index: int = 0,
|
||||
kpt_min_conf: float = 0.30,
|
||||
) -> tuple[float, float, bool]:
|
||||
"""优先 k0；没有可信 k0 时退回框心，并标明 ``is_k0=False``。
|
||||
|
||||
顶视经常看不到腕部视角训出来的 k0。退回框心仍能把臂迁到工件上方，
|
||||
让 D405 重新看见 k0 —— 比假装"已经是 k0"好。调用方必须看第三个返回值。
|
||||
"""
|
||||
uv = k0_uv_from_det(det, kpt_index=kpt_index, kpt_min_conf=kpt_min_conf)
|
||||
if uv is not None:
|
||||
return uv[0], uv[1], True
|
||||
return float(det.cx), float(det.cy), False
|
||||
|
||||
|
||||
def depth_at_m(
|
||||
depth_m: np.ndarray,
|
||||
u: float,
|
||||
v: float,
|
||||
*,
|
||||
win: int = 3,
|
||||
) -> Optional[float]:
|
||||
"""窗口中位数深度 [m]。无效则 None，不许拿 0 去反投影。"""
|
||||
if depth_m is None or depth_m.size == 0:
|
||||
return None
|
||||
h, w = int(depth_m.shape[0]), int(depth_m.shape[1])
|
||||
ui, vi = int(round(u)), int(round(v))
|
||||
r = max(1, int(win))
|
||||
x0, x1 = max(0, ui - r), min(w, ui + r + 1)
|
||||
y0, y1 = max(0, vi - r), min(h, vi + r + 1)
|
||||
if x1 <= x0 or y1 <= y0:
|
||||
return None
|
||||
patch = np.asarray(depth_m[y0:y1, x0:x1], dtype=float).reshape(-1)
|
||||
patch = patch[np.isfinite(patch) & (patch > 1e-4)]
|
||||
if patch.size == 0:
|
||||
return None
|
||||
return float(np.median(patch))
|
||||
|
||||
|
||||
def pixel_on_base_plane(
|
||||
u: float,
|
||||
v: float,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
T_cam_to_base: np.ndarray,
|
||||
plane_z_m: float,
|
||||
) -> Optional[np.ndarray]:
|
||||
"""彩色像素射线和基座水平面 ``z=plane_z_m`` 的交点 [m]。不用深度图。
|
||||
|
||||
相机系方向是 ``((u-cx)/fx, (v-cy)/fy, 1)``，乘外参旋转到基座。
|
||||
交点在相机后面，或射线几乎贴着平面，返回 None。
|
||||
"""
|
||||
T = np.asarray(T_cam_to_base, dtype=float).reshape(4, 4)
|
||||
direction = np.array(
|
||||
[(float(u) - float(cx)) / float(fx), (float(v) - float(cy)) / float(fy), 1.0],
|
||||
dtype=float,
|
||||
)
|
||||
d_base = T[:3, :3] @ direction
|
||||
if abs(float(d_base[2])) < 1e-9:
|
||||
return None
|
||||
scale = (float(plane_z_m) - float(T[2, 3])) / float(d_base[2])
|
||||
if not np.isfinite(scale) or scale <= 0.0:
|
||||
return None
|
||||
point = T[:3, 3] + scale * d_base
|
||||
if not np.all(np.isfinite(point)):
|
||||
return None
|
||||
return np.asarray(point, dtype=float).reshape(3)
|
||||
|
||||
|
||||
def cam_xy_bias_axes(T_cam_to_base: np.ndarray):
|
||||
"""基座水平面上两个单位向量：``(视线前方, 画面右)``。
|
||||
|
||||
视线 = 相机 +Z 投到基座 XY（前倾 30° 时的交平面敏感轴）；
|
||||
画面右 = 相机 +X 投到基座 XY（立体基线 / 图像 u 方向）。
|
||||
水平投影退化时返回 None，不许拿 (0,0) 当轴。
|
||||
"""
|
||||
T = np.asarray(T_cam_to_base, dtype=float).reshape(4, 4)
|
||||
look = T[:3, :3] @ np.array([0.0, 0.0, 1.0], dtype=float)
|
||||
right = T[:3, :3] @ np.array([1.0, 0.0, 0.0], dtype=float)
|
||||
look_xy = np.asarray(look[:2], dtype=float)
|
||||
right_xy = np.asarray(right[:2], dtype=float)
|
||||
n_l = float(np.linalg.norm(look_xy))
|
||||
n_r = float(np.linalg.norm(right_xy))
|
||||
if n_l < 1e-6 or n_r < 1e-6:
|
||||
return None
|
||||
return look_xy / n_l, right_xy / n_r
|
||||
|
||||
|
||||
# 单次精拍这一步必须能走完「框心 → k1」。两孔间距约 60mm，半距 30mm。
|
||||
# 多走多拍的 12mm 封顶在这里会把一半孔距留下（2026-10-13 真机：指令 29mm 只走 12mm）。
|
||||
ONESHOT_ALIGN_CAP_M = 0.040
|
||||
# 下降后补 XY：一跳上限。2026-10-13 两跳合计 4.3mm 把近距检测带崩。
|
||||
ONCE_TRIM_MAX_STEP_M = 0.003
|
||||
ONCE_TRIM_REFUSE_M = 0.008
|
||||
HOLE_PAIR_TRIM_MIN_M = 0.050
|
||||
HOLE_PAIR_TRIM_MAX_M = 0.070
|
||||
# 悬停 → 观察高（约 15mm）单步上限。左臂间隙约 58mm，20mm × 3 步走完。
|
||||
# 观察高 → 贴面不再走这段，改斜线一步（XY 补偿 + 剩余 Z）。
|
||||
ONCE_APPROACH_STEP_M = 0.020
|
||||
# 悬停已对上时 ΔXY 必须为 0。site ``once_ff_look_per_m`` 同源。
|
||||
# 2026-10-14 下午：框心改平面交点后两条链路下臂都仍 +1.3mm X 过补插不进。
|
||||
# k=0.018 是深度框心时代的对冲，不是下降几何。笛卡尔 MoveL 只降 Z。
|
||||
ONCE_FF_LOOK_PER_M = 0.0
|
||||
ONCE_FF_MOVEL_VEL_MPS = 0.05
|
||||
|
||||
|
||||
def once_look_ff_xy(look_xy, descent_m: float, k: float):
|
||||
"""视线前馈 ΔXY [m]。k≤0 是笛卡尔锁 XY：返回 (0,0)，不是 None。
|
||||
|
||||
k>0 时沿 -look 走 k×下降。下降≤0 / 视线退化 → None。
|
||||
"""
|
||||
if float(descent_m) <= 1e-9:
|
||||
return None
|
||||
if float(k) <= 1e-9:
|
||||
return np.array([0.0, 0.0], dtype=float)
|
||||
look = np.asarray(look_xy, dtype=float).reshape(2)
|
||||
n = float(np.linalg.norm(look))
|
||||
if n < 1e-6 or not np.all(np.isfinite(look)):
|
||||
return None
|
||||
look = look / n
|
||||
dxy = -look * float(k) * float(descent_m)
|
||||
if not np.all(np.isfinite(dxy)):
|
||||
return None
|
||||
return dxy
|
||||
|
||||
|
||||
def once_ff_tcp_xyz(
|
||||
flange_xy,
|
||||
tip_off_xy,
|
||||
look_xy,
|
||||
descent_m: float,
|
||||
k: float,
|
||||
surface_z_m: float,
|
||||
gap_m: float,
|
||||
):
|
||||
"""规尖（厂家 MoveL 的 TCP）目标 [m]。Z 是面 + 间隙，不是法兰。"""
|
||||
dxy = once_look_ff_xy(look_xy, descent_m, k)
|
||||
if dxy is None:
|
||||
return None
|
||||
off = np.asarray(tip_off_xy, dtype=float).reshape(2)
|
||||
fl = np.asarray(flange_xy, dtype=float).reshape(2)
|
||||
if not np.all(np.isfinite(off)) or not np.all(np.isfinite(fl)):
|
||||
return None
|
||||
tcp = np.array(
|
||||
[
|
||||
float(fl[0]) + float(off[0]) + float(dxy[0]),
|
||||
float(fl[1]) + float(off[1]) + float(dxy[1]),
|
||||
float(surface_z_m) + float(gap_m),
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
if not np.all(np.isfinite(tcp)):
|
||||
return None
|
||||
return tcp, dxy
|
||||
|
||||
|
||||
def d435_movel_from_tcp(tcp_xyz, flange_xy, k0_xy):
|
||||
"""D435 粗定位 MoveL 目标 [m]：当前厂家 TCP + (K0−法兰) 的 ROS XY。
|
||||
|
||||
keep_z 由硬件用当前 TCP 的 Z 覆盖。ROS 绝对 K0 不能当 WORLD 目标
|
||||
（2026-10-14 贴面 MoveL 已踩过）。缺数 / 非有限 → None。
|
||||
"""
|
||||
try:
|
||||
t = np.asarray(tcp_xyz, dtype=float).reshape(3)
|
||||
fl = np.asarray(flange_xy, dtype=float).reshape(2)
|
||||
k0 = np.asarray(k0_xy, dtype=float).reshape(2)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
if t.size != 3 or fl.size != 2 or k0.size != 2:
|
||||
return None
|
||||
if not np.all(np.isfinite(t)) or not np.all(np.isfinite(fl)) or not np.all(np.isfinite(k0)):
|
||||
return None
|
||||
out = np.array(
|
||||
[float(t[0]) + float(k0[0]) - float(fl[0]),
|
||||
float(t[1]) + float(k0[1]) - float(fl[1]),
|
||||
float(t[2])],
|
||||
dtype=float,
|
||||
)
|
||||
if not np.all(np.isfinite(out)):
|
||||
return None
|
||||
return out
|
||||
|
||||
|
||||
def once_ff_movel_from_tcp(tcp_xyz, dxy, descent_m: float):
|
||||
"""厂家 MoveL 目标 = 当前 Studio TCP + (ΔXY, −下降) [m]。
|
||||
|
||||
2026-10-14：把 ROS 规尖绝对坐标发给 MoveL，k=0.018 与 0.035 都落到
|
||||
WORLD (0.595, −0.468, 0.024)，法兰实走约 (+9, −8)mm。WORLD 原点与
|
||||
ROS 基座不是同一点，绝对坐标不能用。相对位移才跟 k 有关。
|
||||
"""
|
||||
if float(descent_m) <= 1e-9:
|
||||
return None
|
||||
t = np.asarray(tcp_xyz, dtype=float).reshape(3)
|
||||
d = np.asarray(dxy, dtype=float).reshape(2)
|
||||
if t.size != 3 or d.size != 2:
|
||||
return None
|
||||
if not np.all(np.isfinite(t)) or not np.all(np.isfinite(d)):
|
||||
return None
|
||||
out = np.array(
|
||||
[float(t[0]) + float(d[0]), float(t[1]) + float(d[1]), float(t[2]) - float(descent_m)],
|
||||
dtype=float,
|
||||
)
|
||||
if not np.all(np.isfinite(out)):
|
||||
return None
|
||||
return out
|
||||
|
||||
|
||||
def oneshot_align_step_m(err_m: float) -> float:
|
||||
"""单次精拍允许的步长 [m]。不超过半孔距余量；超过 cap 才截断。"""
|
||||
e = abs(float(err_m))
|
||||
cap = float(ONESHOT_ALIGN_CAP_M)
|
||||
if e <= cap:
|
||||
return e
|
||||
return cap
|
||||
|
||||
|
||||
def hole_pair_span_m(p0, p1) -> float:
|
||||
"""两孔基座 XY 间距 [m]。"""
|
||||
a = np.asarray(p0, dtype=float).reshape(-1)
|
||||
b = np.asarray(p1, dtype=float).reshape(-1)
|
||||
return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
|
||||
|
||||
|
||||
def hole_pair_span_ok(span_m: float) -> bool:
|
||||
"""下降后补孔用的间距窗。真机 k1/k2 约 60mm；45mm 是近距检测漂了。"""
|
||||
s = float(span_m)
|
||||
return HOLE_PAIR_TRIM_MIN_M <= s <= HOLE_PAIR_TRIM_MAX_M
|
||||
|
||||
|
||||
def flange_z_for_gap_m(tip_len_m: float, surface_z_m: float, gap_m: float) -> float:
|
||||
"""规尖离面 ``gap_m`` 时的法兰 Z [m]。与轻降几何同一条式子。"""
|
||||
return float(tip_len_m) + float(surface_z_m) + float(gap_m)
|
||||
|
||||
|
||||
def final_gap_refusal(
|
||||
gap_m: float,
|
||||
tool_len_m: float,
|
||||
surface_z_m: float,
|
||||
hover_z_m: float,
|
||||
z_min_m: float,
|
||||
z_max_m: float,
|
||||
) -> str:
|
||||
"""笛卡尔锁 XY 的贴面间隙能不能用。空串表示可以；非空就是拒绝原因。
|
||||
|
||||
参数是规尖离面 [m]，不是法兰 Z：法兰 = 工具长 + 工件面 + 间隙，只在这里算一次。
|
||||
"""
|
||||
gap = float(gap_m)
|
||||
if not np.isfinite(gap):
|
||||
return "贴面间隙不是有限数，拒绝开跑"
|
||||
if gap <= 1e-9:
|
||||
return (
|
||||
f"贴面间隙 {gap * 1000:.1f}mm ≤ 0，规尖会进工件/贴面接触，拒绝开跑"
|
||||
)
|
||||
tool = float(tool_len_m)
|
||||
surf = float(surface_z_m)
|
||||
if tool <= 0.0 or surf <= 0.0:
|
||||
return "写了贴面间隙，但工具长度或工件面未知，无法确认尖端在面之上，拒绝开跑"
|
||||
hover_clear = float(hover_z_m) - tool - surf
|
||||
if gap > hover_clear + 1e-6:
|
||||
return (
|
||||
f"贴面间隙 {gap * 1000:.1f}mm 比悬停离面 {hover_clear * 1000:.1f}mm 还高，"
|
||||
"锁 XY 只会抬臂，拒绝开跑"
|
||||
)
|
||||
flange = flange_z_for_gap_m(tool, surf, gap)
|
||||
z_lo = float(z_min_m)
|
||||
z_hi = float(z_max_m)
|
||||
if flange < z_lo - 1e-6 or flange > z_hi + 1e-6:
|
||||
return (
|
||||
f"贴面法兰 Z {flange:.3f}m（规尖离面 {gap * 1000:.1f}mm）"
|
||||
f"超出工作盒 Z [{z_lo:.3f}, {z_hi:.3f}]，拒绝开跑"
|
||||
)
|
||||
return ""
|
||||
|
||||
|
||||
def decompose_base_xy(delta_xy, T_cam_to_base: np.ndarray):
|
||||
"""把基座 XY 残差拆成沿视线 / 沿画面右 [m]。投影退化返回 None。"""
|
||||
axes = cam_xy_bias_axes(T_cam_to_base)
|
||||
if axes is None:
|
||||
return None
|
||||
look_xy, right_xy = axes
|
||||
d = np.asarray(delta_xy, dtype=float).reshape(2)
|
||||
return {
|
||||
"along_look_m": float(np.dot(d, look_xy)),
|
||||
"along_right_m": float(np.dot(d, right_xy)),
|
||||
"look_xy": [float(look_xy[0]), float(look_xy[1])],
|
||||
"right_xy": [float(right_xy[0]), float(right_xy[1])],
|
||||
}
|
||||
|
||||
|
||||
def px_to_cam(
|
||||
u: float, v: float, z: float, fx: float, fy: float, cx: float, cy: float
|
||||
) -> np.ndarray:
|
||||
return np.array(
|
||||
[(u - cx) * z / fx, (v - cy) * z / fy, z], dtype=float
|
||||
)
|
||||
|
||||
|
||||
def oneshot_k0_base(
|
||||
det,
|
||||
depth_m: np.ndarray,
|
||||
*,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
T_cam_to_base: np.ndarray,
|
||||
min_z_m: float,
|
||||
max_z_m: float,
|
||||
kpt_index: int = 0,
|
||||
kpt_min_conf: float = 0.30,
|
||||
min_det_conf: float = 0.5,
|
||||
color_wh: Optional[tuple[int, int]] = None,
|
||||
depth_wh: Optional[tuple[int, int]] = None,
|
||||
) -> Optional[tuple[np.ndarray, bool]]:
|
||||
"""D435 一张图 → 基座系 XYZ。失败返回 None。
|
||||
|
||||
外参必须是 **cam→基座**（eye_to_hand）。传入腕部 T 会得到错位置且不报错，
|
||||
所以调用方必须自己保证文件对；本函数不再猜。
|
||||
反投影用**彩色图**像素 + 彩色图内参；深度只在对齐图上取样。
|
||||
"""
|
||||
if det is None or float(getattr(det, "conf", 0.0)) < float(min_det_conf):
|
||||
return None
|
||||
u, v, is_k0 = workpiece_uv_from_det(
|
||||
det, kpt_index=kpt_index, kpt_min_conf=kpt_min_conf
|
||||
)
|
||||
u_d, v_d = u, v
|
||||
if color_wh is not None and depth_wh is not None:
|
||||
u_d, v_d = map_uv_to_depth(u, v, color_wh, depth_wh)
|
||||
z = depth_at_m(depth_m, u_d, v_d)
|
||||
if z is None or z < float(min_z_m) or z > float(max_z_m):
|
||||
return None
|
||||
p_cam = px_to_cam(u, v, z, fx, fy, cx, cy)
|
||||
T_ee = np.eye(4) # eye_to_hand 不用法兰，占位
|
||||
p_base = cam_point_to_base(p_cam, T_cam_to_base, T_ee, "eye_to_hand")
|
||||
if not np.all(np.isfinite(p_base)):
|
||||
return None
|
||||
return np.asarray(p_base, dtype=float).reshape(3), bool(is_k0)
|
||||
|
||||
|
||||
@dataclass
|
||||
class StillLock:
|
||||
"""D405 跟住时，法兰 XY 连续 ``still_sec`` 秒位移不超过 ``still_move_m`` → 锁定。
|
||||
|
||||
顶视一次性迁过去的那段路**不算**锁定：``tracking_ok`` 必须是腕部有效目标。
|
||||
大幅一动就重计时，避免"晃一下刚好满 3 秒"误进二阶段。
|
||||
"""
|
||||
|
||||
still_sec: float
|
||||
still_move_m: float
|
||||
_t0: Optional[float] = None
|
||||
_anchor_xy: Optional[np.ndarray] = None
|
||||
|
||||
def reset(self) -> None:
|
||||
self._t0 = None
|
||||
self._anchor_xy = None
|
||||
|
||||
def update(
|
||||
self,
|
||||
p_ee_xy: Optional[np.ndarray],
|
||||
now: float,
|
||||
*,
|
||||
tracking_ok: bool,
|
||||
) -> bool:
|
||||
if (not tracking_ok) or p_ee_xy is None:
|
||||
self.reset()
|
||||
return False
|
||||
xy = np.asarray(p_ee_xy, dtype=float).reshape(2)
|
||||
if self._anchor_xy is None or self._t0 is None:
|
||||
self._anchor_xy = xy.copy()
|
||||
self._t0 = float(now)
|
||||
return False
|
||||
if float(np.linalg.norm(xy - self._anchor_xy)) > float(self.still_move_m):
|
||||
self._anchor_xy = xy.copy()
|
||||
self._t0 = float(now)
|
||||
return False
|
||||
return (float(now) - float(self._t0)) >= float(self.still_sec)
|
||||
@@ -0,0 +1,556 @@
|
||||
"""路径弧长上的梯形速度，以及 XY 圆弧。
|
||||
|
||||
二阶段下臂和一阶段 D435 迁臂共用这一份。不许再抄一套加减速。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
# 整段同一巡航：site ``v_slow_xy`` × 本系数。1.5 → 120 mm/s（远处 hop 上限）。
|
||||
D435_MOVE_SPEED_SCALE = 1.5
|
||||
# D435 厂家 MoveL：SDK ``vel`` [m/s]、``acc`` [m/s²]。
|
||||
# 厂家示例自由空间 0.6 / 近接触 0.2；本档 0.3 和 1.5，短行程也能爬到巡航。
|
||||
# 只改 vel、不给 acc 时，短 hop 仍被默认加速度卡住，肉眼看不出。
|
||||
D435_VENDOR_MOVEL_VEL_MPS = 0.3
|
||||
D435_VENDOR_MOVEL_ACC_MPS2 = 1.5
|
||||
# 缺路程时的兜底加速度。真机 hop 用 ``trap_acc_for_hop``，和二阶段同一条刹车。
|
||||
D435_MOVE_ACC_MPS2 = 0.1875
|
||||
# action 走完后再停这么久，才重测 TF。二阶段 ``_goto`` 的 settle_s。
|
||||
D435_SETTLE_S = 0.25
|
||||
# 冻结雅可比一次只信这么远。二阶段 ``_plan_and_send`` 默认 ``max_seg`` 同源。
|
||||
# 400 mm 一次规划，真机剩 117 mm，随后 28 mm 死循环刷轨迹。
|
||||
FROZEN_J_HOP_MAX_M = 0.08
|
||||
# 剩差进入这一带 = 工件上方。真机最后一跳 37–75 mm @ 80 mm/s 会晃并偏转。
|
||||
D435_NEAR_LEFTOVER_M = 0.08
|
||||
D435_NEAR_HOP_MAX_M = 0.015
|
||||
D435_NEAR_CRUISE_MPS = 0.045
|
||||
# 法兰倾角超过这个才单独收垂直。和 XY 捆在同一跳会把腕部漏成 Y 过冲。
|
||||
D435_VERT_TILT_RAD = 0.015
|
||||
D435_MID_LEFTOVER_M = 0.15
|
||||
D435_MID_HOP_MAX_M = 0.040
|
||||
# 弦长的 12% 当矢高，封顶 50 mm，避免大圆弧甩出工作盒。
|
||||
D435_ARC_SAGITTA_FRAC = 0.12
|
||||
D435_ARC_SAGITTA_MAX_M = 0.05
|
||||
# 整段一次走完。起步必须够长：40 mm 就爬到 80 mm/s 是真机那下弹射
|
||||
# （头尾速度本来就是 0，不是初速度；平移也不下发角速度）。
|
||||
# 峰值加速度 a_max = π v² / (4 d_acc)；150 mm @ 90 mm/s → 约 0.042 m/s²。
|
||||
# 巡航相对原 60 mm/s 加 50%；加减速路程同比加长，起步时间差不多。
|
||||
D435_FULL_ACC_M = 0.150
|
||||
D435_FULL_DEC_M = 0.120
|
||||
D435_FULL_CRUISE_MPS = 0.090
|
||||
|
||||
|
||||
def hybrid_cruise_vxy(
|
||||
v_slow_xy: float, scale: float = D435_MOVE_SPEED_SCALE
|
||||
) -> float:
|
||||
"""远处巡航。近处由 ``oneshot_hop_limits`` 另给一档，不许再抄 15 mm/s。"""
|
||||
return max(1e-4, float(v_slow_xy) * float(scale))
|
||||
|
||||
|
||||
def oneshot_hop_limits(
|
||||
leftover_m: float, far_cruise_mps: float = 0.08
|
||||
) -> Tuple[float, float]:
|
||||
"""这一跳的 ``(hop_max_m, cruise_mps)``。近处必须比 80 mm / 80 mm/s 短且慢。"""
|
||||
d = max(0.0, float(leftover_m))
|
||||
far = max(1e-4, float(far_cruise_mps))
|
||||
if d <= D435_NEAR_LEFTOVER_M:
|
||||
return D435_NEAR_HOP_MAX_M, min(D435_NEAR_CRUISE_MPS, far)
|
||||
if d <= D435_MID_LEFTOVER_M:
|
||||
return D435_MID_HOP_MAX_M, far
|
||||
return FROZEN_J_HOP_MAX_M, far
|
||||
|
||||
|
||||
def flange_down_w(
|
||||
R,
|
||||
*,
|
||||
dead_rad: float = 0.006,
|
||||
gain: float = 2.0,
|
||||
mag_lim: float = 0.5,
|
||||
) -> Tuple[np.ndarray, float]:
|
||||
"""让法兰 +Z 指向世界 −Z 的角速度 + 当前倾角。
|
||||
|
||||
和二阶段 ``_w_down`` 同一份。死区 0.006 rad（约 0.34°）以下不再修。
|
||||
"""
|
||||
z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
|
||||
z_des = np.array([0.0, 0.0, -1.0], dtype=float)
|
||||
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
|
||||
ang = float(math.acos(c))
|
||||
if ang < float(dead_rad):
|
||||
return np.zeros(3, dtype=float), ang
|
||||
axis = np.cross(z_now, z_des)
|
||||
n = float(np.linalg.norm(axis))
|
||||
axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0], dtype=float)
|
||||
mag = float(np.clip(float(gain) * ang, -float(mag_lim), float(mag_lim)))
|
||||
return axis * mag, ang
|
||||
|
||||
|
||||
def flange_tilt_rad(R) -> float:
|
||||
"""法兰 +Z 相对世界 −Z 的倾角 [rad]。只读角度，不带死区。"""
|
||||
z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
|
||||
z_des = np.array([0.0, 0.0, -1.0], dtype=float)
|
||||
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
|
||||
return float(math.acos(c))
|
||||
|
||||
|
||||
def trap_acc_for_hop(dist: float, v: float) -> float:
|
||||
"""二阶段 ``_plan_and_send`` 同一条刹车：路程越长刹车越远，不许另写一套。"""
|
||||
v_cmd = max(min(float(v), 0.08), 1e-3)
|
||||
d_brake = min(0.030, max(0.006, 0.32 * max(float(dist), 1e-6)))
|
||||
return max(v_cmd * v_cmd / (2.0 * d_brake), 0.030)
|
||||
|
||||
|
||||
def trap_params(dist: float, v: float, a: float) -> Tuple[float, float, float, float, float]:
|
||||
"""Rest-to-rest 梯形：返回 (T, v_peak, t_acc, t_cruise, a)。巡航速度保持 v。"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
v = max(float(v), 1e-4)
|
||||
a = max(float(a), 1e-3)
|
||||
d_acc = v * v / (2.0 * a)
|
||||
if 2.0 * d_acc >= dist:
|
||||
v_peak = math.sqrt(max(a * dist, 1e-12))
|
||||
t_acc = v_peak / a
|
||||
return 2.0 * t_acc, v_peak, t_acc, 0.0, a
|
||||
t_acc = v / a
|
||||
t_cruise = (dist - 2.0 * d_acc) / v
|
||||
return 2.0 * t_acc + t_cruise, v, t_acc, t_cruise, a
|
||||
|
||||
|
||||
def trap_sv(
|
||||
t: float, dist: float, v_peak: float, t_acc: float, t_cruise: float, a: float,
|
||||
) -> Tuple[float, float]:
|
||||
"""时刻 t 的路径弧长 s 与切向速度。"""
|
||||
t = max(0.0, float(t))
|
||||
t_dec0 = t_acc + t_cruise
|
||||
t_end = t_dec0 + t_acc
|
||||
if t >= t_end:
|
||||
return dist, 0.0
|
||||
if t < t_acc:
|
||||
return 0.5 * a * t * t, a * t
|
||||
if t < t_dec0:
|
||||
return 0.5 * a * t_acc * t_acc + v_peak * (t - t_acc), v_peak
|
||||
td = t - t_dec0
|
||||
s0 = 0.5 * a * t_acc * t_acc + v_peak * t_cruise
|
||||
return s0 + v_peak * td - 0.5 * a * td * td, max(0.0, v_peak - a * td)
|
||||
|
||||
|
||||
def smooth_path_params(
|
||||
dist: float,
|
||||
v: float,
|
||||
*,
|
||||
d_acc: float = D435_FULL_ACC_M,
|
||||
d_dec: float = D435_FULL_DEC_M,
|
||||
) -> Tuple[float, float, float, float, float, float]:
|
||||
"""整段一次走完的余弦加减速：(T, v_peak, t_acc, t_cruise, d_acc, d_dec)。
|
||||
|
||||
加速/减速段速度按半个余弦走，两端加速度为 0，没有梯形那种加速度台阶。
|
||||
路程不够巡航时整段一条余弦，峰值不超过 ``v``。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
v = max(float(v), 1e-4)
|
||||
acc = min(max(float(d_acc), 1e-4), 0.45 * dist)
|
||||
dec = min(max(float(d_dec), 1e-4), 0.45 * dist)
|
||||
if acc + dec >= dist - 1e-6:
|
||||
# 整段一条正弦速度：v = v_peak sin(π t / T)，s(T)=2 v_peak T / π。
|
||||
T = math.pi * dist / (2.0 * v)
|
||||
return T, v, 0.0, 0.0, 0.0, dist
|
||||
t_acc = 2.0 * acc / v
|
||||
t_dec = 2.0 * dec / v
|
||||
t_cruise = (dist - acc - dec) / v
|
||||
T = t_acc + t_cruise + t_dec
|
||||
return T, v, t_acc, t_cruise, acc, dec
|
||||
|
||||
|
||||
def cosine_peak_acc(v: float, d_acc: float) -> float:
|
||||
"""余弦加速段的峰值切向加速度 [m/s²]。``d_acc`` 越长，起步越不弹。"""
|
||||
d = max(float(d_acc), 1e-6)
|
||||
return math.pi * float(v) * float(v) / (4.0 * d)
|
||||
|
||||
|
||||
def smooth_path_sv(
|
||||
t: float,
|
||||
dist: float,
|
||||
v_peak: float,
|
||||
t_acc: float,
|
||||
t_cruise: float,
|
||||
d_acc: float,
|
||||
d_dec: float,
|
||||
) -> Tuple[float, float]:
|
||||
"""时刻 t 的弧长与切向速度。头尾速度为 0。"""
|
||||
t = max(0.0, float(t))
|
||||
dist = max(float(dist), 1e-6)
|
||||
v_peak = max(float(v_peak), 1e-4)
|
||||
if t_acc <= 1e-9 and t_cruise <= 1e-9:
|
||||
T = math.pi * dist / (2.0 * v_peak)
|
||||
if t >= T:
|
||||
return dist, 0.0
|
||||
u = math.pi * t / T
|
||||
s = (v_peak * T / math.pi) * (1.0 - math.cos(u))
|
||||
vs = v_peak * math.sin(u)
|
||||
return min(s, dist), max(0.0, vs)
|
||||
t_dec = 2.0 * float(d_dec) / v_peak if v_peak > 1e-9 else 0.0
|
||||
t_end = float(t_acc) + float(t_cruise) + t_dec
|
||||
if t >= t_end:
|
||||
return dist, 0.0
|
||||
if t < t_acc:
|
||||
u = math.pi * t / t_acc
|
||||
s = 0.5 * v_peak * (t - t_acc / math.pi * math.sin(u))
|
||||
vs = 0.5 * v_peak * (1.0 - math.cos(u))
|
||||
return s, vs
|
||||
if t < t_acc + t_cruise:
|
||||
return float(d_acc) + v_peak * (t - t_acc), v_peak
|
||||
td = t - t_acc - t_cruise
|
||||
u = math.pi * td / t_dec if t_dec > 1e-9 else math.pi
|
||||
s0 = float(d_acc) + v_peak * float(t_cruise)
|
||||
s = s0 + 0.5 * v_peak * (td + t_dec / math.pi * math.sin(u))
|
||||
vs = 0.5 * v_peak * (1.0 + math.cos(u))
|
||||
return min(s, dist), max(0.0, vs)
|
||||
|
||||
|
||||
def _circle_3(
|
||||
a: np.ndarray, b: np.ndarray, c: np.ndarray
|
||||
) -> Optional[Tuple[np.ndarray, float]]:
|
||||
ax, ay = float(a[0]), float(a[1])
|
||||
bx, by = float(b[0]), float(b[1])
|
||||
cx, cy = float(c[0]), float(c[1])
|
||||
d = 2.0 * (ax * (by - cy) + bx * (cy - ay) + cx * (ay - by))
|
||||
if abs(d) < 1e-9:
|
||||
return None
|
||||
a2 = ax * ax + ay * ay
|
||||
b2 = bx * bx + by * by
|
||||
c2 = cx * cx + cy * cy
|
||||
ux = (a2 * (by - cy) + b2 * (cy - ay) + c2 * (ay - by)) / d
|
||||
uy = (a2 * (cx - bx) + b2 * (ax - cx) + c2 * (bx - ax)) / d
|
||||
center = np.array([ux, uy], dtype=float)
|
||||
r = float(np.hypot(ax - ux, ay - uy))
|
||||
if r < 1e-4:
|
||||
return None
|
||||
return center, r
|
||||
|
||||
|
||||
def _ccw(a0: float, a1: float) -> float:
|
||||
return (a1 - a0) % (2.0 * math.pi)
|
||||
|
||||
|
||||
def _xy_in_box(xy: np.ndarray, lo: np.ndarray, hi: np.ndarray) -> bool:
|
||||
return bool(
|
||||
float(lo[0]) <= float(xy[0]) <= float(hi[0])
|
||||
and float(lo[1]) <= float(xy[1]) <= float(hi[1])
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class ArcXyMove:
|
||||
"""从 p0 走到 p1 的 XY 圆弧（或退化成直线），弧长上跑梯形速度。"""
|
||||
|
||||
p0: np.ndarray
|
||||
p1: np.ndarray
|
||||
length: float
|
||||
v_cruise: float
|
||||
T: float
|
||||
v_peak: float
|
||||
t_acc: float
|
||||
t_cruise: float
|
||||
a: float
|
||||
t0: float
|
||||
linear: bool
|
||||
center: Optional[np.ndarray] = None
|
||||
radius: float = 0.0
|
||||
ang0: float = 0.0
|
||||
sweep: float = 0.0
|
||||
s_last: float = 0.0
|
||||
_dir: np.ndarray = field(default_factory=lambda: np.array([1.0, 0.0]))
|
||||
|
||||
def sample(self, now: float) -> Tuple[np.ndarray, np.ndarray, bool]:
|
||||
"""返回 (计划 XY, 切向速度, 是否走完)。"""
|
||||
t = max(0.0, float(now) - float(self.t0))
|
||||
s, sdot = trap_sv(
|
||||
t, self.length, self.v_peak, self.t_acc, self.t_cruise, self.a
|
||||
)
|
||||
s = min(float(s), float(self.length))
|
||||
self.s_last = s
|
||||
done = t >= float(self.T) - 1e-4 or s >= float(self.length) - 1e-5
|
||||
if self.linear or self.radius <= 1e-6 or self.center is None:
|
||||
L = max(float(self.length), 1e-6)
|
||||
u = s / L
|
||||
xy = (1.0 - u) * self.p0 + u * self.p1
|
||||
vxy = self._dir * float(sdot)
|
||||
return xy, vxy, done
|
||||
frac = 0.0 if abs(self.sweep) < 1e-9 else s / (self.radius * abs(self.sweep))
|
||||
frac = min(1.0, max(0.0, frac))
|
||||
ang = self.ang0 + self.sweep * frac
|
||||
xy = self.center + self.radius * np.array(
|
||||
[math.cos(ang), math.sin(ang)], dtype=float
|
||||
)
|
||||
tang = np.array([-math.sin(ang), math.cos(ang)], dtype=float)
|
||||
if self.sweep < 0.0:
|
||||
tang = -tang
|
||||
return xy, tang * float(sdot), done
|
||||
|
||||
|
||||
def build_arc_xy_move(
|
||||
p0,
|
||||
p1,
|
||||
*,
|
||||
v: float,
|
||||
a: float,
|
||||
now: float,
|
||||
ws_min=None,
|
||||
ws_max=None,
|
||||
sag_frac: float = D435_ARC_SAGITTA_FRAC,
|
||||
sag_max: float = D435_ARC_SAGITTA_MAX_M,
|
||||
) -> ArcXyMove:
|
||||
"""p0→p1 的圆弧。工作盒装不下就换侧，再不行走直线。速度整段同一档。"""
|
||||
p0 = np.asarray(p0, dtype=float).reshape(2)
|
||||
p1 = np.asarray(p1, dtype=float).reshape(2)
|
||||
chord = p1 - p0
|
||||
L = float(np.hypot(chord[0], chord[1]))
|
||||
direction = (
|
||||
chord / L if L > 1e-6 else np.array([1.0, 0.0], dtype=float)
|
||||
)
|
||||
cruise = max(float(v), 1e-4)
|
||||
acc = max(float(a), 1e-3)
|
||||
|
||||
def _linear() -> ArcXyMove:
|
||||
T, v_peak, t_acc, t_cruise, a_use = trap_params(max(L, 1e-6), cruise, acc)
|
||||
return ArcXyMove(
|
||||
p0=p0.copy(),
|
||||
p1=p1.copy(),
|
||||
length=max(L, 1e-6),
|
||||
v_cruise=cruise,
|
||||
T=T,
|
||||
v_peak=v_peak,
|
||||
t_acc=t_acc,
|
||||
t_cruise=t_cruise,
|
||||
a=a_use,
|
||||
t0=float(now),
|
||||
linear=True,
|
||||
_dir=direction,
|
||||
)
|
||||
|
||||
if L < 0.003:
|
||||
return _linear()
|
||||
|
||||
n = np.array([-direction[1], direction[0]], dtype=float)
|
||||
h = min(float(sag_max), float(sag_frac) * L)
|
||||
mid = 0.5 * (p0 + p1)
|
||||
lo = None if ws_min is None else np.asarray(ws_min, dtype=float).reshape(-1)[:2]
|
||||
hi = None if ws_max is None else np.asarray(ws_max, dtype=float).reshape(-1)[:2]
|
||||
|
||||
def _try(sign: float) -> Optional[ArcXyMove]:
|
||||
apex = mid + n * (h * sign)
|
||||
cir = _circle_3(p0, apex, p1)
|
||||
if cir is None:
|
||||
return None
|
||||
center, r = cir
|
||||
a0 = math.atan2(p0[1] - center[1], p0[0] - center[0])
|
||||
a1 = math.atan2(p1[1] - center[1], p1[0] - center[0])
|
||||
a_via = math.atan2(apex[1] - center[1], apex[0] - center[0])
|
||||
ccw_end = _ccw(a0, a1)
|
||||
ccw_via = _ccw(a0, a_via)
|
||||
if 1e-6 < ccw_via < ccw_end - 1e-6:
|
||||
sweep = ccw_end
|
||||
else:
|
||||
sweep = ccw_end - 2.0 * math.pi
|
||||
if abs(sweep) < 1e-6:
|
||||
return None
|
||||
length = r * abs(sweep)
|
||||
if lo is not None and hi is not None:
|
||||
for k in range(9):
|
||||
frac = k / 8.0
|
||||
ang = a0 + sweep * frac
|
||||
xy = center + r * np.array([math.cos(ang), math.sin(ang)])
|
||||
if not _xy_in_box(xy, lo, hi):
|
||||
return None
|
||||
T, v_peak, t_acc, t_cruise, a_use = trap_params(length, cruise, acc)
|
||||
return ArcXyMove(
|
||||
p0=p0.copy(),
|
||||
p1=p1.copy(),
|
||||
length=length,
|
||||
v_cruise=cruise,
|
||||
T=T,
|
||||
v_peak=v_peak,
|
||||
t_acc=t_acc,
|
||||
t_cruise=t_cruise,
|
||||
a=a_use,
|
||||
t0=float(now),
|
||||
linear=False,
|
||||
center=center,
|
||||
radius=r,
|
||||
ang0=a0,
|
||||
sweep=sweep,
|
||||
)
|
||||
|
||||
for sign in (1.0, -1.0):
|
||||
got = _try(sign)
|
||||
if got is not None:
|
||||
return got
|
||||
return _linear()
|
||||
|
||||
|
||||
def path_interp(waypoints, u: float):
|
||||
"""沿关节路点折线插值。u∈[0,1] → (q, dq/du)。"""
|
||||
nseg = len(waypoints) - 1
|
||||
if nseg <= 0:
|
||||
z = np.zeros_like(waypoints[0])
|
||||
return waypoints[0].copy(), z
|
||||
u = float(min(1.0, max(0.0, u)))
|
||||
x = u * nseg
|
||||
i = min(nseg - 1, int(math.floor(x)))
|
||||
a = x - i
|
||||
q = (1.0 - a) * waypoints[i] + a * waypoints[i + 1]
|
||||
dq_du = (waypoints[i + 1] - waypoints[i]) * nseg
|
||||
return q, dq_du
|
||||
|
||||
|
||||
def translate_hold_ori_waypoints(
|
||||
q0,
|
||||
J,
|
||||
dp,
|
||||
*,
|
||||
damping: float = 0.08,
|
||||
max_dq: float = 0.06,
|
||||
R=None,
|
||||
verticalize: bool = False,
|
||||
):
|
||||
"""冻结当前雅可比，把 ``dp`` 拆成关节路点。
|
||||
|
||||
和二阶段 ``_plan_and_send`` 同一份逆解。``verticalize`` 时顺带收法兰朝下，
|
||||
不许另抄一份 ``_w_down``。返回 ``(路点, 还剩多少米)``。
|
||||
"""
|
||||
from arm_application.geom_ik import dls_translate_hold_ori, dls_twist
|
||||
|
||||
q = np.asarray(q0, dtype=float).copy()
|
||||
remain = np.asarray(dp, dtype=float).reshape(3).copy()
|
||||
dist = float(np.linalg.norm(remain))
|
||||
wps = [q.copy()]
|
||||
inner_tol = min(0.00012, max(0.00005, 0.05 * max(dist, 1e-6)))
|
||||
n_max = int(math.ceil(dist / 0.008)) + 16
|
||||
Jj = np.asarray(J, dtype=float)
|
||||
w_step = np.zeros(3, dtype=float)
|
||||
ang = 0.0
|
||||
if verticalize:
|
||||
if R is None:
|
||||
raise ValueError("verticalize 必须传入当前法兰 R，不许假装已经朝下")
|
||||
w_step, ang = flange_down_w(R)
|
||||
for _ in range(n_max):
|
||||
if float(np.linalg.norm(remain)) < inner_tol and (
|
||||
(not verticalize) or ang < 0.006
|
||||
):
|
||||
break
|
||||
nseg = max(1, int(math.ceil(float(np.linalg.norm(remain)) / 0.012)))
|
||||
step = remain / nseg
|
||||
if verticalize and ang > 0.008:
|
||||
qdot = dls_twist(Jj, step / 0.12, w_base=w_step, damping=damping, max_qdot=0.45)
|
||||
dq = np.clip(qdot * 0.12, -float(max_dq), float(max_dq))
|
||||
else:
|
||||
dq = dls_translate_hold_ori(Jj, step, damping=damping, max_dq=max_dq)
|
||||
if float(np.max(np.abs(dq))) < 1e-5:
|
||||
break
|
||||
q = q + dq
|
||||
wps.append(q.copy())
|
||||
remain = remain - (Jj[:3, :] @ dq)
|
||||
ang = max(0.0, ang - float(np.linalg.norm(w_step)) * 0.12)
|
||||
return wps, float(np.linalg.norm(remain))
|
||||
|
||||
|
||||
def trap_joint_schedule(
|
||||
waypoints,
|
||||
dist: float,
|
||||
v: float,
|
||||
a: float,
|
||||
*,
|
||||
t0: float = 0.06,
|
||||
dt: float = 0.04,
|
||||
t_hold: float = 0.25,
|
||||
):
|
||||
"""路点 + 梯形速度 → ``[(q, qdot, t), ...]``。头尾速度为 0，时间严格递增。
|
||||
|
||||
一次下发整段。中途再发下一条会把这条掐掉，臂就重新加速 —— 那就是抖。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
T_move, v_peak, t_acc, t_cruise, a_use = trap_params(dist, v, a)
|
||||
n_samp = max(8, int(math.ceil(T_move / max(float(dt), 1e-3))))
|
||||
zeros = np.zeros_like(waypoints[0])
|
||||
out = []
|
||||
last_t = -1.0
|
||||
for k in range(n_samp + 1):
|
||||
t_local = T_move * k / n_samp
|
||||
s, vs = trap_sv(t_local, dist, v_peak, t_acc, t_cruise, a_use)
|
||||
u = s / dist
|
||||
qi, dq_du = path_interp(waypoints, u)
|
||||
qdot = dq_du * (vs / dist) if k < n_samp else zeros
|
||||
if k >= n_samp:
|
||||
qi = waypoints[-1]
|
||||
qdot = zeros
|
||||
t = float(t0) + t_local
|
||||
if t <= last_t:
|
||||
t = last_t + 0.02
|
||||
out.append((np.asarray(qi, dtype=float).copy(), np.asarray(qdot, dtype=float).copy(), t))
|
||||
last_t = t
|
||||
hold = max(0.12, float(t_hold))
|
||||
for frac in (0.40, 0.70, 1.00):
|
||||
t = float(t0) + T_move + hold * frac
|
||||
if t < last_t + 0.05:
|
||||
t = last_t + 0.05
|
||||
out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
|
||||
last_t = t
|
||||
return out, T_move, v_peak
|
||||
|
||||
|
||||
def smooth_joint_schedule(
|
||||
waypoints,
|
||||
dist: float,
|
||||
v: float,
|
||||
*,
|
||||
t0: float = 0.06,
|
||||
dt: float = 0.04,
|
||||
t_hold: float = 0.25,
|
||||
d_acc: float = D435_FULL_ACC_M,
|
||||
d_dec: float = D435_FULL_DEC_M,
|
||||
):
|
||||
"""路点 + 余弦加减速 → ``[(q, qdot, t), ...]``。头尾速度为 0。
|
||||
|
||||
D435 整段一次下发用这一份。二阶段短跳仍走 ``trap_joint_schedule``。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
T_move, v_peak, t_acc, t_cruise, acc_m, dec_m = smooth_path_params(
|
||||
dist, v, d_acc=d_acc, d_dec=d_dec
|
||||
)
|
||||
n_samp = max(12, int(math.ceil(T_move / max(float(dt), 1e-3))))
|
||||
zeros = np.zeros_like(waypoints[0])
|
||||
out = []
|
||||
last_t = -1.0
|
||||
for k in range(n_samp + 1):
|
||||
t_local = T_move * k / n_samp
|
||||
s, vs = smooth_path_sv(
|
||||
t_local, dist, v_peak, t_acc, t_cruise, acc_m, dec_m
|
||||
)
|
||||
u = min(1.0, s / dist)
|
||||
qi, dq_du = path_interp(waypoints, u)
|
||||
qdot = dq_du * (vs / dist) if k < n_samp else zeros
|
||||
if k >= n_samp:
|
||||
qi = waypoints[-1]
|
||||
qdot = zeros
|
||||
t = float(t0) + t_local
|
||||
if t <= last_t:
|
||||
t = last_t + 0.02
|
||||
out.append(
|
||||
(
|
||||
np.asarray(qi, dtype=float).copy(),
|
||||
np.asarray(qdot, dtype=float).copy(),
|
||||
t,
|
||||
)
|
||||
)
|
||||
last_t = t
|
||||
hold = max(0.12, float(t_hold))
|
||||
for frac in (0.40, 0.70, 1.00):
|
||||
t = float(t0) + T_move + hold * frac
|
||||
if t < last_t + 0.05:
|
||||
t = last_t + 0.05
|
||||
out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
|
||||
last_t = t
|
||||
return out, T_move, v_peak
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,64 @@
|
||||
#!/usr/bin/env python3
|
||||
"""PID 控制器，带积分限幅、微分作用于测量值（derivative-on-measurement）以及积分分离。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional
|
||||
|
||||
|
||||
class PidController:
|
||||
"""``u = Kp e + Ki ∫e + Kd (−dmeas/dt)``，然后再做饱和限幅。
|
||||
|
||||
积分分离：当 ``|error| > i_sep_thresh`` 时，积分被强制置 0 且不再累积
|
||||
（只用 P+D，以便快速追赶 / 减少超调）。
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
kp: float,
|
||||
ki: float,
|
||||
kd: float,
|
||||
*,
|
||||
i_limit: float = 0.05,
|
||||
out_limit: float = 0.20,
|
||||
i_sep_thresh: float = 0.005,
|
||||
) -> None:
|
||||
self.kp = float(kp)
|
||||
self.ki = float(ki)
|
||||
self.kd = float(kd)
|
||||
self.i_limit = abs(float(i_limit))
|
||||
self.out_limit = abs(float(out_limit))
|
||||
self.i_sep_thresh = abs(float(i_sep_thresh))
|
||||
self._i = 0.0
|
||||
self._meas_prev: Optional[float] = None
|
||||
|
||||
def reset(self) -> None:
|
||||
self._i = 0.0
|
||||
self._meas_prev = None
|
||||
|
||||
@property
|
||||
def integral(self) -> float:
|
||||
return float(self._i)
|
||||
|
||||
def update(self, error: float, meas: float, dt: float) -> float:
|
||||
dt = max(float(dt), 1e-4)
|
||||
err = float(error)
|
||||
x = float(meas)
|
||||
if self._meas_prev is None:
|
||||
dterm = 0.0
|
||||
else:
|
||||
dterm = -(x - self._meas_prev) / dt
|
||||
self._meas_prev = x
|
||||
|
||||
# 积分分离 —— 误差较大：清空 I，不再累积
|
||||
if self.i_sep_thresh > 0.0 and abs(err) > self.i_sep_thresh:
|
||||
self._i = 0.0
|
||||
else:
|
||||
self._i = max(-self.i_limit, min(self.i_limit, self._i + err * dt))
|
||||
|
||||
u = self.kp * err + self.ki * self._i + self.kd * dterm
|
||||
if u > self.out_limit:
|
||||
u = self.out_limit
|
||||
elif u < -self.out_limit:
|
||||
u = -self.out_limit
|
||||
return float(u)
|
||||
@@ -0,0 +1,441 @@
|
||||
#!/usr/bin/env python3
|
||||
"""跟踪锁定 → 孔定位。搜孔轻压已取消，本节点不再下压搜孔。
|
||||
|
||||
流畅性（相对 bash 杀进程再起节点）：
|
||||
- 相机 / 触觉 / 手眼 TF 全程只起一次
|
||||
- 一阶段锁定后 SIGTERM 跟踪节点，立刻 pin 当前关节，不回零、不抬臂
|
||||
- 二阶段已垂直 / 已在悬停高则跳过对应动作；对准完就停，不再下压搜孔
|
||||
- 旋入不在这个节点里
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import subprocess
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PointStamped
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
from std_msgs.msg import Bool, String
|
||||
|
||||
from arm_application.stage3_search_node import Stage3Search
|
||||
|
||||
|
||||
#: 悬停时规尖离面不够，跟踪高再抬的量 [m]。
|
||||
#:
|
||||
#: 只在「悬停高上规尖离工件面 < :data:`TRACK_LOCK_GAP_M`」时加上。仿真悬停按
|
||||
#: 工具几何只留约 2mm，不抬就会在跟踪时刮到工件。真机 89 悬停 0.320 时规尖
|
||||
#: 已经在面上方约 75mm，再抬 50mm 会让对孔前先改一次高度（0.37→0.32）。
|
||||
#:
|
||||
#: ⚠️ 抽成常量是因为它不只被流程用：``scripts/check_sim_scene.py`` 要用它核对
|
||||
#: "跟踪高有没有超出工作盒上界"（仿真里悬停 0.632 + 0.05 = 0.682 > 真机那套
|
||||
#: 0.65 上界，就是被这条查出来的）。两处各写一个字面量迟早漂移。
|
||||
TRACK_CLEARANCE_M = 0.05
|
||||
#: 悬停时规尖离面已经不少于这个值 [m]，一阶段就锁在悬停高，对孔完成后再下降。
|
||||
TRACK_LOCK_GAP_M = 0.040
|
||||
|
||||
|
||||
def init_z_refusal(
|
||||
z_m: float,
|
||||
tool_len_m: float,
|
||||
surface_z_m: float,
|
||||
gap_m: float,
|
||||
z_min: float,
|
||||
z_max: float,
|
||||
) -> str:
|
||||
"""初始法兰高度能不能用。空串表示可以；非空就是拒绝原因，不许悄悄抬高。"""
|
||||
z = float(z_m)
|
||||
if z < float(z_min) - 1e-6 or z > float(z_max) + 1e-6:
|
||||
return (
|
||||
f"初始法兰 Z {z:.3f}m 超出工作盒 [{float(z_min):.3f}, {float(z_max):.3f}]，"
|
||||
"拒绝开跑"
|
||||
)
|
||||
tool = float(tool_len_m)
|
||||
surf = float(surface_z_m)
|
||||
if tool <= 0.0 or surf <= 0.0:
|
||||
return "写了初始高度，但工具长度或工件面未知，无法确认尖端在面之上，拒绝开跑"
|
||||
clear = z - tool - surf
|
||||
if clear < float(gap_m) - 0.001:
|
||||
return (
|
||||
f"初始法兰 Z {z:.3f}m 会让工具尖进入工件："
|
||||
f"尖端 {z - tool:.3f}，工件面 {surf:.3f}，"
|
||||
f"间隙 {clear * 1000:.1f}mm，要求 ≥ {float(gap_m) * 1000:.1f}mm。"
|
||||
"拒绝开跑，不会悄悄抬高。"
|
||||
)
|
||||
return ""
|
||||
|
||||
|
||||
def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
|
||||
"""一阶段锁高。
|
||||
|
||||
悬停时规尖离面已经 ≥ ``TRACK_LOCK_GAP_M``：锁在悬停高，跟踪和对孔都不改 Z，
|
||||
对孔完成后再下降。离面不够（仿真悬停只留 2mm）：再抬 ``TRACK_CLEARANCE_M``，
|
||||
否则跟踪会刮到工件。工具长或工件面缺失时抬升 —— 缺数时锁在悬停高等于没检查。
|
||||
"""
|
||||
tool = float(tool_len_m)
|
||||
surf = float(surface_z_m)
|
||||
hover = float(hover_z)
|
||||
if tool > 0.0 and surf > 0.0 and (hover - tool - surf) >= TRACK_LOCK_GAP_M:
|
||||
return hover
|
||||
return hover + TRACK_CLEARANCE_M
|
||||
|
||||
|
||||
def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
|
||||
"""一阶段超时了，到底是"感知没给点"还是"给了点但没收敛"。
|
||||
|
||||
这两种情况的排查方向完全相反，而现象长得一模一样（都是"没锁定"）：
|
||||
|
||||
* **一个点都没收到** → 感知侧的事：yolo 后端多半缺 ultralytics/torch、
|
||||
truth 后端真值节点没起来、external 后端没人发。跟标定、跟工件在不在
|
||||
视野里**都没关系**。
|
||||
* **收到了点但没收敛** → 控制/几何侧的事：工件不在视野、手眼不对、
|
||||
目标超出工作空间、增益太小。
|
||||
|
||||
以前两种情况共用一句"没锁定（视野里没工件？）"，于是"perception_node
|
||||
因为缺依赖早退了"被读成"算法不收敛"，白查半天。
|
||||
"""
|
||||
if points_seen <= 0:
|
||||
return (
|
||||
f"一阶段 {timeout_s:.0f}s 没锁定：**一个视觉目标点都没收到**"
|
||||
"（visual_servo/filtered_point 上 0 条）—— 这是感知没起来，"
|
||||
"不是标定/视野问题。按 perception.source 对号入座：\n"
|
||||
" yolo → 多半缺推理依赖（ultralytics/torch），"
|
||||
f"看感知进程退出前几行：{log_path}\n"
|
||||
" truth → 仿真真值节点没起来，或场景/物体名读不出来"
|
||||
"（它退出前会打印原因）\n"
|
||||
" external → 外部节点没按这个话题/这个类型发"
|
||||
)
|
||||
return (
|
||||
f"一阶段 {timeout_s:.0f}s 没锁定（收到 {points_seen} 个目标点但没收敛）："
|
||||
f"工件不在视野 / 手眼不对 / 目标超出工作空间。看 {log_path}"
|
||||
)
|
||||
|
||||
|
||||
class AssemblyPipeline(Stage3Search):
|
||||
def __init__(self, node_name: str = "assembly_pipeline") -> None:
|
||||
# node_name 可覆盖：通止规装配（gauge_assembly_node）继承本类以复用
|
||||
# 一阶段跟踪的拉起/拆分与二阶段定位链，但它是**另一条流程**，
|
||||
# 在图上必须是另一个节点名（否则 ros2 node list 里两个同名的节点，
|
||||
# 出问题时连"是哪条流程在发指令"都分不清）。
|
||||
super().__init__(node_name=node_name)
|
||||
self.declare_parameter("skip_stage1", False)
|
||||
self.declare_parameter("skip_stage2", False)
|
||||
self.declare_parameter("skip_stage3", True) # 搜孔轻压已取消，写成 false 也不再下压
|
||||
self.declare_parameter("d435_move_only", False)
|
||||
self.declare_parameter("init_z_m", -1.0) # >0：启动后先到这个法兰高度
|
||||
self.declare_parameter("stage1_timeout_sec", 150.0)
|
||||
self.linger3 = 1.2 # 串联结束短保持；单独跑 stage3 仍用 site linger_sec
|
||||
|
||||
self._s1: Optional[subprocess.Popen] = None
|
||||
self._locked = False
|
||||
self.create_subscription(Bool, "/visual_servo/locked", self._on_lock, 10)
|
||||
# 诊断用：一阶段超时时靠它区分"感知没给点"和"给了点没收敛"。
|
||||
# 只计数，不参与控制 —— 它绝不能影响流程本身的判断。
|
||||
#
|
||||
# ⚠️ QoS 必须与感知发布端**逐项一致**（BEST_EFFORT + KEEP_LAST(5)，
|
||||
# 见 perception_node / sim_truth_perception）。用默认的 RELIABLE 订阅
|
||||
# BEST_EFFORT 的话题：不报错、话题列表里也在，但**一条都收不到** ——
|
||||
# 于是超时归因会一口咬定"感知没起来"，把人往完全错误的方向带
|
||||
#（真实原因可能只是这里 QoS 不匹配）。这类"诊断自己坏了"最难查，
|
||||
# 所以宁可多写这几行。
|
||||
self.declare_parameter("diag_point_topic", "visual_servo/filtered_point")
|
||||
self._diag_point_topic = str(self.get_parameter("diag_point_topic").value)
|
||||
self._point_msgs = 0
|
||||
self.create_subscription(
|
||||
PointStamped,
|
||||
self._diag_point_topic,
|
||||
self._on_point_seen,
|
||||
QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
),
|
||||
)
|
||||
self._pipe_pub = self.create_publisher(String, "/pipeline/state", 10)
|
||||
self._pipe_result = self.create_publisher(String, "/pipeline/result", 10)
|
||||
|
||||
skip1 = bool(self.get_parameter("skip_stage1").value)
|
||||
skip2 = bool(self.get_parameter("skip_stage2").value)
|
||||
skip3 = bool(self.get_parameter("skip_stage3").value)
|
||||
self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
|
||||
self.get_logger().info(
|
||||
f"pipeline arm_ee={self.ee_frame} hole={self.target_hole} "
|
||||
f"skip1={skip1} skip2={skip2} skip3={skip3} "
|
||||
f"d435_only={self.d435_move_only} insert={self.insert_depth_mm:.1f}mm"
|
||||
)
|
||||
|
||||
def _state(self, s: str) -> None:
|
||||
self.get_logger().info(f"[pipeline] === {s} ===")
|
||||
msg = String()
|
||||
msg.data = s
|
||||
self._state_pub.publish(msg)
|
||||
self._pipe_pub.publish(msg)
|
||||
|
||||
def _on_lock(self, msg: Bool) -> None:
|
||||
if bool(msg.data):
|
||||
self._locked = True
|
||||
|
||||
def _on_point_seen(self, _msg: PointStamped) -> None:
|
||||
"""只记"收到过目标点"，用于超时后的归因（见 stage1_timeout_reason）。"""
|
||||
self._point_msgs += 1
|
||||
|
||||
def _stop_stage1(self) -> None:
|
||||
proc = self._s1
|
||||
self._s1 = None
|
||||
if proc is None or proc.poll() is not None:
|
||||
return
|
||||
from arm_application.process_lifecycle import stop_process_tree
|
||||
stop_process_tree(proc)
|
||||
|
||||
def _track_z(self) -> float:
|
||||
"""一阶段锁高。写了 ``init_z_m`` 就用它，否则见 :func:`track_lock_z`。"""
|
||||
z = float(self.get_parameter("init_z_m").value)
|
||||
if z > 0.0:
|
||||
return z
|
||||
return track_lock_z(
|
||||
float(self.hover_z),
|
||||
float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
|
||||
float(getattr(self, "surface_z_m", 0.0) or 0.0),
|
||||
)
|
||||
|
||||
def _ensure_clearance(self) -> bool:
|
||||
"""没写初始高度：只在低于锁高时抬上去。写了：升或降到那个法兰 Z，不安全就拒绝。"""
|
||||
user = float(self.get_parameter("init_z_m").value) > 0.0
|
||||
q, Tee = self._wait_state(4.0)
|
||||
if q is None or Tee is None:
|
||||
if user:
|
||||
self.get_logger().error("设了 --init-z，但拿不到当前位姿，拒绝开跑")
|
||||
return False
|
||||
return True
|
||||
tz = self._track_z()
|
||||
if user:
|
||||
why = init_z_refusal(
|
||||
tz,
|
||||
float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
|
||||
float(getattr(self, "surface_z_m", 0.0) or 0.0),
|
||||
float(self.final_gap_m),
|
||||
float(self.ws_min[2]),
|
||||
float(self.ws_max[2]),
|
||||
)
|
||||
if why:
|
||||
self.get_logger().error(why)
|
||||
return False
|
||||
z = float(Tee[2, 3])
|
||||
if user:
|
||||
if abs(z - tz) <= 0.008:
|
||||
self.get_logger().info(f"已在初始高度 Z={z:.3f}m（目标 {tz:.3f}）")
|
||||
return True
|
||||
elif z >= tz - 0.008:
|
||||
return True
|
||||
self._state("pre_lift")
|
||||
self.get_logger().info(f"先到初始高度：Z {z:.3f} → {tz:.3f}m")
|
||||
ok, q, Tee = self._goto(
|
||||
q, Tee, np.array([float(Tee[0, 3]), float(Tee[1, 3]), tz]),
|
||||
verticalize=True, tol_m=0.003, tol_tilt=self.vert_tol_rad * 2.0,
|
||||
tag="pre_lift", v=self.move_speed, max_hops=8, hold_xy=True, settle_s=0.25,
|
||||
)
|
||||
q2 = self._q()
|
||||
self._pin(q2 if q2 is not None else q)
|
||||
z2 = float(Tee[2, 3])
|
||||
if user and (not ok or abs(z2 - tz) > 0.008):
|
||||
self.get_logger().error(f"初始高度没到位：Z={z2:.3f}，目标 {tz:.3f}")
|
||||
return False
|
||||
return True
|
||||
|
||||
def _run_stage1(self) -> int:
|
||||
"""拉起 perception+servo，等到 /visual_servo/locked 后冻结并拆跟踪。"""
|
||||
if not self._ensure_clearance():
|
||||
return 2
|
||||
self._state("stage1_track")
|
||||
self._locked = False
|
||||
log_path = "/tmp/pipeline_stage1.log"
|
||||
logf = open(log_path, "w", encoding="utf-8")
|
||||
lock_z = self._track_z()
|
||||
# use_sim_time 必须**原样传下去**：本节点在仿真里是 true，子 launch 若是
|
||||
# false，perception/servo 就跑在系统时间上，TF 全是仿真时刻的戳 →
|
||||
# "看得见图、算得出点，就是查不到变换"，一阶段永远不收敛。
|
||||
use_sim = bool(self.get_parameter("use_sim_time").value)
|
||||
cmd = [
|
||||
"ros2", "launch", "arm_application", "visual_servo.launch.py",
|
||||
"enable_control:=true",
|
||||
"publish_handeye_tf:=false",
|
||||
f"ee_z_m:={lock_z:.3f}",
|
||||
f"use_sim_time:={str(use_sim).lower()}",
|
||||
f"d435_move_only:={str(self.d435_move_only).lower()}",
|
||||
]
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
f"启动 D435 位移测试（锁高 {lock_z:.3f}m，到位后不交 D405、不进二阶段）"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
|
||||
)
|
||||
self._s1 = subprocess.Popen(
|
||||
cmd, stdout=logf, stderr=subprocess.STDOUT, start_new_session=False,
|
||||
)
|
||||
timeout = float(self.get_parameter("stage1_timeout_sec").value)
|
||||
self._point_msgs = 0
|
||||
t0 = time.monotonic()
|
||||
while time.monotonic() - t0 < timeout:
|
||||
self._spin(0.1)
|
||||
if self._locked:
|
||||
break
|
||||
if self._s1.poll() is not None:
|
||||
self.get_logger().error(f"一阶段进程提前退出，看 {log_path}")
|
||||
return 1
|
||||
else:
|
||||
self._stop_stage1()
|
||||
self.get_logger().error(
|
||||
stage1_timeout_reason(self._point_msgs, timeout, log_path)
|
||||
)
|
||||
return 1
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
"D435 位移测试结束 — 停跟踪、钉住当前关节，跳过二阶段"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
|
||||
self._stop_stage1()
|
||||
time.sleep(0.35)
|
||||
q = self._q()
|
||||
if q is not None:
|
||||
self._pin(q)
|
||||
return 0
|
||||
|
||||
def run(self) -> int:
|
||||
skip1 = bool(self.get_parameter("skip_stage1").value)
|
||||
skip2 = bool(self.get_parameter("skip_stage2").value)
|
||||
if not bool(self.get_parameter("skip_stage3").value):
|
||||
self.get_logger().info("搜孔轻压已取消，忽略要求重新打开它的参数")
|
||||
skip3 = True
|
||||
if self.d435_move_only:
|
||||
skip2 = True
|
||||
skip3 = True
|
||||
if not self._cli.wait_for_server(timeout_sec=5.0):
|
||||
self.get_logger().error("JTC action 不在 —— 先开 T1: run_real.sh --arm 89")
|
||||
return 2
|
||||
|
||||
try:
|
||||
if not skip1:
|
||||
code = self._run_stage1()
|
||||
if code != 0:
|
||||
return code
|
||||
else:
|
||||
if not self._ensure_clearance():
|
||||
return 2
|
||||
self._state("skip_stage1")
|
||||
|
||||
q, Tee = self._wait_state(10.0)
|
||||
if q is None or Tee is None:
|
||||
self.get_logger().error("拿不到 joint_states/TF")
|
||||
return 2
|
||||
|
||||
final: dict = {}
|
||||
if not skip2:
|
||||
self._state("stage2_locate")
|
||||
code, q, Tee, final = self._run_locate(q, Tee)
|
||||
if code != 0:
|
||||
self.get_logger().error(f"二阶段失败 exit={code}，停在当前位姿")
|
||||
return code
|
||||
self.get_logger().info(
|
||||
f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
|
||||
" — 搜孔已取消，停在这里。旋入请另跑。"
|
||||
)
|
||||
else:
|
||||
self._state("skip_stage2")
|
||||
|
||||
if skip3:
|
||||
self._state("skip_stage3")
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
"D435 位移测试结束 —— 臂停在当前高度，不交 D405、不对孔；"
|
||||
"15s 后本进程退出，臂保持不动"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
"搜孔已取消 —— 臂停在当前高度。"
|
||||
"15s 后本进程退出，臂保持不动"
|
||||
)
|
||||
if final:
|
||||
self._publish_result(final)
|
||||
self._linger_hold(final if final else {"ok": True}, seconds=15.0)
|
||||
payload = {"ok": True, "stage": "pipeline", "exit": 0, "skipped": "stage3"}
|
||||
try:
|
||||
payload["stage2"] = json.loads(
|
||||
open("/tmp/stage2_result.json", encoding="utf-8").read()
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
msg = String()
|
||||
msg.data = json.dumps(payload, ensure_ascii=False)
|
||||
self._pipe_result.publish(msg)
|
||||
return 0
|
||||
|
||||
self._state("stage3_search")
|
||||
code = self._run_search(q, Tee, linger=False, stage2_final=final)
|
||||
payload = {
|
||||
"ok": code == 0,
|
||||
"stage": "pipeline",
|
||||
"exit": code,
|
||||
}
|
||||
try:
|
||||
payload["stage3"] = json.loads(
|
||||
open("/tmp/stage3_result.json", encoding="utf-8").read()
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
msg = String()
|
||||
msg.data = json.dumps(payload, ensure_ascii=False)
|
||||
self._pipe_result.publish(msg)
|
||||
return code
|
||||
finally:
|
||||
self._stop_stage1()
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = AssemblyPipeline()
|
||||
code = 2
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
node.get_logger().info("用户中断")
|
||||
code = 130
|
||||
except Exception as exc: # noqa: BLE001
|
||||
node.get_logger().error(f"pipeline 异常终止: {exc}")
|
||||
code = 2
|
||||
finally:
|
||||
try:
|
||||
node._stop_stage1()
|
||||
except Exception:
|
||||
pass
|
||||
if code != 0:
|
||||
try:
|
||||
q = node._q()
|
||||
if q is not None:
|
||||
node._pin(q)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
node._shutdown_tf()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
node.destroy_node()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
except Exception:
|
||||
pass
|
||||
os._exit(code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,300 @@
|
||||
#!/usr/bin/env python3
|
||||
"""在 YOLO 检测框中心处求立体平面的 Z。
|
||||
|
||||
悬停目标是「检测框中心射线」与「拟合出的近平面」的交点。这样既能留在检测结果上，
|
||||
又带有真实的米制 Z（30° 相机下，Z 错了会把 base 系的 XY 也带偏）。不使用点云均值
|
||||
质心 —— 孔 / 桌面的内点会把它从绿框上拉走。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class PlaneFeature:
|
||||
uv: np.ndarray # (2,) 三维质心对应的彩色像素坐标
|
||||
z: float # 三维质心在相机坐标系下的 Z
|
||||
xyz: np.ndarray # (3,) 相机坐标系下的立体点云质心
|
||||
n_inliers: int
|
||||
note: str
|
||||
# 这个点的 Z 是不是**借**来的（该像素处读不到自己的深度，从检测框里借了一个）。
|
||||
# 为什么要显式带出来、而不是让下游去认 ``note`` 里的字：借深度意味着该像素的
|
||||
# 深度不可读 ⇒ 绝对位置不足以用来**重定位**（2026-10-08 真机，假 k0 带着借来的
|
||||
# 深度、z 看着很合理 0.27~0.39m，于是"物理量程"守卫也失效）。
|
||||
borrowed: bool = False
|
||||
|
||||
|
||||
def _depth_meters(buf: np.ndarray, encoding: str) -> Tuple[np.ndarray, float]:
|
||||
enc = (encoding or "").lower()
|
||||
raw = np.asarray(buf)
|
||||
if enc in ("16uc1", "mono16") or raw.dtype == np.uint16:
|
||||
pos = raw.reshape(-1).astype(np.float32)
|
||||
pos = pos[pos > 0]
|
||||
if pos.size >= 8:
|
||||
med = float(np.median(pos))
|
||||
scale = 0.001 if 20.0 <= med <= 8000.0 else (1.0 if med < 20.0 else 0.0001)
|
||||
else:
|
||||
scale = 0.001
|
||||
return raw.astype(np.float32) * scale, scale
|
||||
return raw.astype(np.float32), 1.0
|
||||
|
||||
|
||||
def _near_band_mask(
|
||||
meters: np.ndarray,
|
||||
valid: np.ndarray,
|
||||
min_depth: float,
|
||||
work_z_max: float,
|
||||
band_m: float,
|
||||
) -> np.ndarray:
|
||||
vals = meters[valid]
|
||||
if vals.size < 8:
|
||||
return valid
|
||||
vals = np.sort(vals)
|
||||
n_near = max(8, int(0.40 * vals.size))
|
||||
near = vals[:n_near]
|
||||
z_p10 = float(np.percentile(near, 10))
|
||||
hi = min(float(work_z_max), z_p10 + float(band_m))
|
||||
return valid & (meters <= hi) & (meters >= float(min_depth))
|
||||
|
||||
|
||||
def _ransac_plane(
|
||||
pts: np.ndarray,
|
||||
thresh: float,
|
||||
n_iter: int = 24,
|
||||
rng: Optional[np.random.Generator] = None,
|
||||
) -> Optional[Tuple[np.ndarray, np.ndarray]]:
|
||||
n = int(pts.shape[0])
|
||||
if n < 12:
|
||||
return None
|
||||
rng = rng or np.random.default_rng()
|
||||
best_inl: Optional[np.ndarray] = None
|
||||
best_count = 0
|
||||
for _ in range(n_iter):
|
||||
i = rng.choice(n, 3, replace=False)
|
||||
p0, p1, p2 = pts[i]
|
||||
nvec = np.cross(p1 - p0, p2 - p0)
|
||||
nn = float(np.linalg.norm(nvec))
|
||||
if nn < 1e-9:
|
||||
continue
|
||||
nvec = nvec / nn
|
||||
dist = np.abs((pts - p0) @ nvec)
|
||||
inl = dist < thresh
|
||||
c = int(inl.sum())
|
||||
if c > best_count:
|
||||
best_count = c
|
||||
best_inl = inl
|
||||
if best_inl is None or best_count < max(12, int(0.15 * n)):
|
||||
return None
|
||||
subset = pts[best_inl]
|
||||
p0 = subset.mean(axis=0)
|
||||
_, _, vh = np.linalg.svd(subset - p0, full_matrices=False)
|
||||
nvec = vh[-1]
|
||||
nvec = nvec / (float(np.linalg.norm(nvec)) + 1e-12)
|
||||
if float(nvec @ p0) > 0.0:
|
||||
nvec = -nvec
|
||||
return nvec, p0
|
||||
|
||||
|
||||
def _plane_z(
|
||||
nvec: np.ndarray,
|
||||
p0: np.ndarray,
|
||||
u: float,
|
||||
v: float,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
) -> Optional[float]:
|
||||
direc = np.array([(u - cx) / max(fx, 1.0), (v - cy) / max(fy, 1.0), 1.0], dtype=float)
|
||||
denom = float(nvec @ direc)
|
||||
if abs(denom) < 1e-8:
|
||||
return None
|
||||
z = float(nvec @ p0) / denom
|
||||
return z if np.isfinite(z) else None
|
||||
|
||||
|
||||
def workpiece_xyz_in_box(
|
||||
depth_buf: np.ndarray,
|
||||
encoding: str,
|
||||
xyxy,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
img_wh: Tuple[int, int],
|
||||
min_depth: float,
|
||||
max_depth: float,
|
||||
work_z_max: float,
|
||||
inner: float = 0.40,
|
||||
band_m: float = 0.06,
|
||||
) -> Optional[PlaneFeature]:
|
||||
"""工件上的实体点：内层检测框里近表面深度的三维中位数。
|
||||
|
||||
不是单目射线（孔会穿透到桌面），也不是整个 ROI 的点云均值
|
||||
（桌面 / 单侧深度会把它从工件上拉走）。
|
||||
"""
|
||||
meters, _ = _depth_meters(depth_buf, encoding)
|
||||
if meters.ndim != 2:
|
||||
return None
|
||||
img_w, img_h = int(img_wh[0]), int(img_wh[1])
|
||||
dh, dw = int(meters.shape[0]), int(meters.shape[1])
|
||||
x1, y1, x2, y2 = [float(v) for v in xyxy]
|
||||
u_c = 0.5 * (x1 + x2)
|
||||
v_c = 0.5 * (y1 + y2)
|
||||
if dw != img_w or dh != img_h:
|
||||
sx, sy = dw / max(float(img_w), 1.0), dh / max(float(img_h), 1.0)
|
||||
x1, y1, x2, y2 = x1 * sx, y1 * sy, x2 * sx, y2 * sy
|
||||
scale_u, scale_v = 1.0 / sx, 1.0 / sy
|
||||
else:
|
||||
scale_u = scale_v = 1.0
|
||||
bw = max(8.0, x2 - x1)
|
||||
bh = max(8.0, y2 - y1)
|
||||
frac = 0.5 * float(np.clip(inner, 0.2, 0.8))
|
||||
ix0 = int(np.clip(min(x1, x2) + (0.5 - frac) * bw, 0, dw - 1))
|
||||
iy0 = int(np.clip(min(y1, y2) + (0.5 - frac) * bh, 0, dh - 1))
|
||||
ix1 = int(np.clip(max(x1, x2) - (0.5 - frac) * bw, ix0 + 2, dw))
|
||||
iy1 = int(np.clip(max(y1, y2) - (0.5 - frac) * bh, iy0 + 2, dh))
|
||||
roi = meters[iy0:iy1, ix0:ix1]
|
||||
finite = np.isfinite(roi) & (roi > float(min_depth)) & (roi < float(max_depth))
|
||||
if int(finite.sum()) < 12:
|
||||
return None
|
||||
near = _near_band_mask(roi, finite, min_depth, work_z_max, band_m)
|
||||
if int(near.sum()) < 8:
|
||||
near = finite
|
||||
yy, xx = np.nonzero(near)
|
||||
if yy.size < 8:
|
||||
return None
|
||||
zs = roi[yy, xx]
|
||||
us = (xx + ix0) * scale_u
|
||||
vs = (yy + iy0) * scale_v
|
||||
pts = np.stack(
|
||||
[
|
||||
(us - cx) * zs / max(fx, 1.0),
|
||||
(vs - cy) * zs / max(fy, 1.0),
|
||||
zs,
|
||||
],
|
||||
axis=1,
|
||||
)
|
||||
p_xyz = np.median(pts, axis=0)
|
||||
if not np.all(np.isfinite(p_xyz)):
|
||||
return None
|
||||
z = float(p_xyz[2])
|
||||
if not (min_depth <= z <= work_z_max):
|
||||
return None
|
||||
uv = np.array(
|
||||
[
|
||||
fx * float(p_xyz[0]) / max(z, 1e-6) + cx,
|
||||
fy * float(p_xyz[1]) / max(z, 1e-6) + cy,
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
return PlaneFeature(
|
||||
uv=uv,
|
||||
z=z,
|
||||
xyz=p_xyz.astype(float),
|
||||
n_inliers=int(yy.size),
|
||||
note=f"inner3d n={int(yy.size)} z={z:.3f}",
|
||||
)
|
||||
|
||||
|
||||
def geometric_center_on_plane(
|
||||
depth_buf: np.ndarray,
|
||||
encoding: str,
|
||||
xyxy,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
img_wh: Tuple[int, int],
|
||||
min_depth: float,
|
||||
max_depth: float,
|
||||
work_z_max: float,
|
||||
inlier_m: float = 0.008,
|
||||
band_m: float = 0.06,
|
||||
) -> Optional[PlaneFeature]:
|
||||
"""拟合出的近平面上、位于检测框中心的点，拿不到则返回 None。"""
|
||||
meters, _ = _depth_meters(depth_buf, encoding)
|
||||
if meters.ndim != 2:
|
||||
return None
|
||||
img_w, img_h = int(img_wh[0]), int(img_wh[1])
|
||||
dh, dw = int(meters.shape[0]), int(meters.shape[1])
|
||||
x1, y1, x2, y2 = [float(v) for v in xyxy]
|
||||
u_c = 0.5 * (x1 + x2)
|
||||
v_c = 0.5 * (y1 + y2)
|
||||
if dw != img_w or dh != img_h:
|
||||
sx, sy = dw / max(float(img_w), 1.0), dh / max(float(img_h), 1.0)
|
||||
x1, y1, x2, y2 = x1 * sx, y1 * sy, x2 * sx, y2 * sy
|
||||
# uv 保持用彩色像素坐标：先在深度像素下计算，再缩放回来。
|
||||
scale_u, scale_v = 1.0 / sx, 1.0 / sy
|
||||
else:
|
||||
scale_u = scale_v = 1.0
|
||||
|
||||
x0 = int(np.clip(min(x1, x2), 0, dw - 1))
|
||||
y0 = int(np.clip(min(y1, y2), 0, dh - 1))
|
||||
x1i = int(np.clip(max(x1, x2), x0 + 2, dw))
|
||||
y1i = int(np.clip(max(y1, y2), y0 + 2, dh))
|
||||
roi = meters[y0:y1i, x0:x1i]
|
||||
if roi.size < 16:
|
||||
return None
|
||||
|
||||
finite = np.isfinite(roi) & (roi > float(min_depth)) & (roi < float(max_depth))
|
||||
if int(finite.sum()) < 16:
|
||||
return None
|
||||
near = _near_band_mask(roi, finite, min_depth, work_z_max, band_m)
|
||||
if int(near.sum()) < 12:
|
||||
near = finite
|
||||
|
||||
yy, xx = np.nonzero(near)
|
||||
zs = roi[yy, xx]
|
||||
# 反投影用的彩色等效像素（对齐后的深度使用的是彩色内参 K）。
|
||||
us = (xx + x0) * scale_u
|
||||
vs = (yy + y0) * scale_v
|
||||
pts = np.stack(
|
||||
[
|
||||
(us - cx) * zs / max(fx, 1.0),
|
||||
(vs - cy) * zs / max(fy, 1.0),
|
||||
zs,
|
||||
],
|
||||
axis=1,
|
||||
)
|
||||
if pts.shape[0] > 800:
|
||||
rng = np.random.default_rng()
|
||||
sel = rng.choice(pts.shape[0], 800, replace=False)
|
||||
fit_pts = pts[sel]
|
||||
else:
|
||||
fit_pts = pts
|
||||
|
||||
fitted = _ransac_plane(fit_pts, thresh=float(inlier_m))
|
||||
if fitted is None:
|
||||
return None
|
||||
nvec, p0 = fitted
|
||||
dist = np.abs((pts - p0) @ nvec)
|
||||
n_inl = int((dist < float(inlier_m)).sum())
|
||||
if n_inl < 12:
|
||||
return None
|
||||
z = _plane_z(nvec, p0, u_c, v_c, fx, fy, cx, cy)
|
||||
if z is None or not (min_depth <= float(z) <= work_z_max):
|
||||
zs_inl = pts[dist < float(inlier_m), 2]
|
||||
z = float(np.median(zs_inl)) if zs_inl.size else None
|
||||
if z is None or not (min_depth <= z <= work_z_max):
|
||||
return None
|
||||
z = float(z)
|
||||
p_xyz = np.array(
|
||||
[
|
||||
(u_c - cx) * z / max(fx, 1.0),
|
||||
(v_c - cy) * z / max(fy, 1.0),
|
||||
z,
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
return PlaneFeature(
|
||||
uv=np.array([u_c, v_c], dtype=float),
|
||||
z=z,
|
||||
xyz=p_xyz,
|
||||
n_inliers=n_inl,
|
||||
note=f"box_on_plane n={n_inl} z={z:.3f}",
|
||||
)
|
||||
@@ -0,0 +1,479 @@
|
||||
#!/usr/bin/env python3
|
||||
"""YOLO **pose** 模型的推理封装（自解码，不经过 ultralytics 的 pose 后处理）。
|
||||
|
||||
背景：为什么会多出这一个模块
|
||||
--------------------------------
|
||||
`gongjian0929` 这类权重是 ``task=pose``：它输出**一个工件框 + K 个关键点**。
|
||||
本项目 K=3，就是现场说的 k0 / k1 / k2（k0 跟踪、k1/k2 对孔）。
|
||||
|
||||
ultralytics 走不通，两条路都实测过（2026-09-29）：
|
||||
|
||||
1. ``YOLO(engine, task="detect")`` —— 按检测去解 14 通道输出，结果吐出 300 个
|
||||
垃圾框，``conf`` 直接是坐标值（600+），全被算成 ``workpiece`` 类。
|
||||
跟踪会锁到空气上 —— 真机上这比不跑更危险。
|
||||
2. ``YOLO(engine, task="pose")`` —— 直接崩：``negative dimension -983``。
|
||||
根因是 ``scripts/weights.py`` 用**裸 TensorRT API** 构建 engine，engine 不带
|
||||
metadata；ultralytics 拿不到 ``kpt_shape``，后处理就错位了。
|
||||
（TensorRT 10.13 的 Python API 确实没有任何 metadata 接口 —— 见
|
||||
``models/README.md`` 第 3 节。）
|
||||
|
||||
所以这里自己解码。输出布局是 YOLO pose 的固定约定::
|
||||
|
||||
output0 : (1, 4 + nc + 3*K, A) # A = anchor 数（640 输入时是 8400）
|
||||
ch 0..3 : cx, cy, w, h # 输入尺度下的像素
|
||||
ch 4.. : 每类置信度（nc 个）
|
||||
之后 : K 组 (x, y, conf)
|
||||
|
||||
关键点是**同一个工件框的属性**，不是独立目标 —— 所以它们不参与 NMS 的竞争，
|
||||
只跟着自己那个框一起被选出/淘汰。这点和"每个孔一个类"的检测模型不一样。
|
||||
|
||||
后端
|
||||
----
|
||||
- ``.engine``：TensorRT 10 的 ``set_tensor_address`` + ``execute_async_v3``。
|
||||
用 torch 张量当设备缓冲、torch 的当前流当执行流 —— 不需要 pycuda。
|
||||
实测 ~1.5 ms/帧（640 输入，FP16 engine）。
|
||||
- ``.onnx``：onnxruntime。**本环境没有 CUDA provider**（只有 CPU），
|
||||
所以 ONNX 只是兜底，真机请用 engine。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
import logging
|
||||
from pathlib import Path
|
||||
from typing import Any, List, Optional, Sequence, Tuple, Union
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from .yolo_detector import Detection, resolve_class_labels
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# ultralytics 的默认填充值。改了它并不会错 —— 只是输入分布和训练时不一致，
|
||||
# 精度会掉一点，所以别改。
|
||||
LETTERBOX_FILL = 114
|
||||
|
||||
# 关键点语义（现场约定）：k0 是跟踪孔，k1/k2 是对孔。
|
||||
# 默认值写在这里，site.yaml 可以覆盖（见 arm_application/pose_detector 的调用方）。
|
||||
TRACK_KPT_INDEX = 0
|
||||
HOLE_KPT_INDICES: Tuple[int, int] = (1, 2)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 模型信息：engine 没有 metadata，就从旁边同名的 .onnx 读
|
||||
# --------------------------------------------------------------------------- #
|
||||
def _parse_python_literal(raw: Any, default: Any = None) -> Any:
|
||||
"""ONNX metadata 里存的是 python repr（单引号、嵌套 list/dict），要 literal_eval。"""
|
||||
if raw is None:
|
||||
return default
|
||||
if isinstance(raw, (dict, list, tuple, int)):
|
||||
return raw
|
||||
try:
|
||||
return ast.literal_eval(str(raw))
|
||||
except (ValueError, SyntaxError):
|
||||
return default
|
||||
|
||||
|
||||
def read_pose_info(weights: Union[str, Path]) -> Optional[dict]:
|
||||
"""读同名 ``.onnx`` 的 metadata，判断这是不是一个 pose 模型。
|
||||
|
||||
engine 本身读不到任何东西（TensorRT 10 的 Python API 没有 metadata 接口），
|
||||
但 ``.onnx`` 一定在它旁边（``resolve_weights`` 保证同目录同主干），
|
||||
所以统一以 ONNX 为准。
|
||||
|
||||
返回 ``None`` = 不是 pose 模型（调用方应回落到普通检测器）。
|
||||
返回 dict 时至少含 ``kpt_count`` / ``nc`` / ``names``。
|
||||
"""
|
||||
p = Path(weights).expanduser()
|
||||
onnx_path = p.with_suffix(".onnx")
|
||||
if not onnx_path.is_file():
|
||||
return None
|
||||
try:
|
||||
import onnx
|
||||
except Exception: # noqa: BLE001
|
||||
logger.warning("没装 onnx，无法判断 %s 是不是 pose 模型", p.name)
|
||||
return None
|
||||
try:
|
||||
meta = {m.key: m.value for m in onnx.load(str(onnx_path)).metadata_props}
|
||||
except Exception as exc: # noqa: BLE001
|
||||
logger.warning("读 %s 的 metadata 失败：%s", onnx_path.name, exc)
|
||||
return None
|
||||
|
||||
task = str(meta.get("task", "")).strip().lower()
|
||||
kpt_shape = _parse_python_literal(meta.get("kpt_shape"))
|
||||
if task != "pose" and not kpt_shape:
|
||||
return None
|
||||
|
||||
kpt_count = 0
|
||||
if isinstance(kpt_shape, (list, tuple)) and len(kpt_shape) >= 1:
|
||||
try:
|
||||
kpt_count = int(kpt_shape[0])
|
||||
except (TypeError, ValueError):
|
||||
kpt_count = 0
|
||||
if kpt_count <= 0:
|
||||
# metadata 缺 kpt_shape 也不能猜 —— 猜错就是"静默错位"，比报错难查得多。
|
||||
logger.warning("%s 标记为 pose 但读不到 kpt_shape，按非 pose 处理", onnx_path.name)
|
||||
return None
|
||||
|
||||
names = _parse_python_literal(meta.get("names"), default={}) or {}
|
||||
kpt_names = _parse_python_literal(meta.get("kpt_names"), default={}) or {}
|
||||
|
||||
nc = 0
|
||||
if isinstance(names, dict) and names:
|
||||
nc = max(int(k) for k in names) + 1 if all(
|
||||
str(k).lstrip("-").isdigit() for k in names
|
||||
) else len(names)
|
||||
if nc <= 0 and isinstance(names, (list, tuple)):
|
||||
nc = len(names)
|
||||
|
||||
return {
|
||||
"task": "pose",
|
||||
"kpt_count": kpt_count,
|
||||
"kpt_names": kpt_names,
|
||||
"nc": nc,
|
||||
"names": names,
|
||||
"model": str(meta.get("description") or p.name),
|
||||
}
|
||||
|
||||
|
||||
def is_pose_model(weights: Union[str, Path]) -> bool:
|
||||
return read_pose_info(weights) is not None
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 预处理 / 后处理（纯 numpy，可离线单测）
|
||||
# --------------------------------------------------------------------------- #
|
||||
def letterbox(
|
||||
img: np.ndarray,
|
||||
new_shape: int = 640,
|
||||
color: Tuple[int, int, int] = (LETTERBOX_FILL,) * 3,
|
||||
) -> Tuple[np.ndarray, float, Tuple[float, float]]:
|
||||
"""等比缩放 + 居中填充到 ``new_shape``（和 ultralytics 推理时一致）。
|
||||
|
||||
返回 ``(padded, gain, (dw, dh))``：``dw/dh`` 是**填充量**，还原坐标时减掉它。
|
||||
"""
|
||||
h, w = img.shape[:2]
|
||||
gain = min(new_shape / h, new_shape / w)
|
||||
new_unpad = (max(1, int(round(w * gain))), max(1, int(round(h * gain))))
|
||||
if (w, h) != new_unpad:
|
||||
img = cv2.resize(img, new_unpad, interpolation=cv2.INTER_LINEAR)
|
||||
dw = (new_shape - new_unpad[0]) / 2.0
|
||||
dh = (new_shape - new_unpad[1]) / 2.0
|
||||
top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1))
|
||||
left, right = int(round(dw - 0.1)), int(round(dw + 0.1))
|
||||
if top or bottom or left or right:
|
||||
img = cv2.copyMakeBorder(
|
||||
img, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color
|
||||
)
|
||||
return img, float(gain), (float(dw), float(dh))
|
||||
|
||||
|
||||
def nms_xyxy(
|
||||
boxes: np.ndarray,
|
||||
scores: np.ndarray,
|
||||
iou_thr: float,
|
||||
max_det: int = 20,
|
||||
) -> np.ndarray:
|
||||
"""纯 numpy 的 NMS，返回保留下来的下标（按分数从高到低）。"""
|
||||
if boxes.size == 0:
|
||||
return np.zeros((0,), dtype=int)
|
||||
x1, y1, x2, y2 = boxes[:, 0], boxes[:, 1], boxes[:, 2], boxes[:, 3]
|
||||
areas = np.maximum(0.0, x2 - x1) * np.maximum(0.0, y2 - y1)
|
||||
order = scores.argsort()[::-1]
|
||||
keep: List[int] = []
|
||||
while order.size > 0 and len(keep) < int(max_det):
|
||||
i = int(order[0])
|
||||
keep.append(i)
|
||||
if order.size == 1:
|
||||
break
|
||||
rest = order[1:]
|
||||
ix1 = np.maximum(x1[i], x1[rest])
|
||||
iy1 = np.maximum(y1[i], y1[rest])
|
||||
ix2 = np.minimum(x2[i], x2[rest])
|
||||
iy2 = np.minimum(y2[i], y2[rest])
|
||||
inter = np.maximum(0.0, ix2 - ix1) * np.maximum(0.0, iy2 - iy1)
|
||||
union = areas[i] + areas[rest] - inter
|
||||
iou = np.where(union > 1e-9, inter / np.maximum(union, 1e-9), 0.0)
|
||||
order = rest[iou <= float(iou_thr)]
|
||||
return np.asarray(keep, dtype=int)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 后端：TensorRT engine / ONNX Runtime
|
||||
# --------------------------------------------------------------------------- #
|
||||
class _TrtRunner:
|
||||
"""TensorRT 10 执行器：torch 张量当设备缓冲，torch 当前流当执行流。
|
||||
|
||||
不用 pycuda —— 环境里只有 torch，而 ``set_tensor_address`` +
|
||||
``execute_async_v3`` 正好够用。
|
||||
"""
|
||||
|
||||
def __init__(self, engine_path: Path, device: str = "0") -> None:
|
||||
import tensorrt as trt
|
||||
import torch
|
||||
|
||||
self._torch = torch
|
||||
idx = 0
|
||||
dev = str(device or "").strip().lower()
|
||||
if dev and dev not in ("auto", "cuda", "gpu"):
|
||||
tail = dev.split(":")[-1]
|
||||
if tail.isdigit():
|
||||
idx = int(tail)
|
||||
if not torch.cuda.is_available():
|
||||
raise RuntimeError("backend=tensorrt 但 torch 看不到 CUDA")
|
||||
self._device = torch.device(f"cuda:{idx}")
|
||||
|
||||
trt_logger = trt.Logger(trt.Logger.ERROR)
|
||||
with open(engine_path, "rb") as f:
|
||||
engine = trt.Runtime(trt_logger).deserialize_cuda_engine(f.read())
|
||||
if engine is None:
|
||||
raise RuntimeError(
|
||||
f"engine 反序列化失败：{engine_path}。"
|
||||
"engine 和 TensorRT 版本绑死，重装环境后要重新导出"
|
||||
"（bash scripts/weights.sh doctor --check-engines 可查）"
|
||||
)
|
||||
self._engine = engine
|
||||
self._ctx = engine.create_execution_context()
|
||||
|
||||
names = [engine.get_tensor_name(i) for i in range(engine.num_io_tensors)]
|
||||
ins = [n for n in names if engine.get_tensor_mode(n) == trt.TensorIOMode.INPUT]
|
||||
outs = [n for n in names if engine.get_tensor_mode(n) == trt.TensorIOMode.OUTPUT]
|
||||
if len(ins) != 1 or len(outs) != 1:
|
||||
raise RuntimeError(f"pose engine 期望 1 进 1 出，实际 in={ins} out={outs}")
|
||||
self.in_name, self.out_name = ins[0], outs[0]
|
||||
|
||||
with torch.cuda.device(self._device):
|
||||
self._x = torch.zeros(
|
||||
tuple(engine.get_tensor_shape(self.in_name)),
|
||||
dtype=torch.float32,
|
||||
device=self._device,
|
||||
)
|
||||
self._y = torch.zeros(
|
||||
tuple(engine.get_tensor_shape(self.out_name)),
|
||||
dtype=torch.float32,
|
||||
device=self._device,
|
||||
)
|
||||
self._ctx.set_tensor_address(self.in_name, self._x.data_ptr())
|
||||
self._ctx.set_tensor_address(self.out_name, self._y.data_ptr())
|
||||
self.out_shape = tuple(engine.get_tensor_shape(self.out_name))
|
||||
|
||||
def run(self, chw: np.ndarray) -> np.ndarray:
|
||||
torch = self._torch
|
||||
with torch.cuda.device(self._device):
|
||||
self._x.copy_(torch.from_numpy(np.ascontiguousarray(chw)).to(self._device))
|
||||
self._ctx.execute_async_v3(torch.cuda.current_stream().cuda_stream)
|
||||
torch.cuda.current_stream().synchronize()
|
||||
return self._y.cpu().numpy()
|
||||
|
||||
|
||||
class _OnnxRunner:
|
||||
"""onnxruntime 兜底。本环境只有 CPU provider —— 真机请用 .engine。"""
|
||||
|
||||
def __init__(self, onnx_path: Path) -> None:
|
||||
import onnxruntime as ort
|
||||
|
||||
providers = ort.get_available_providers()
|
||||
if "CUDAExecutionProvider" in providers:
|
||||
chosen = ["CUDAExecutionProvider", "CPUExecutionProvider"]
|
||||
else:
|
||||
logger.warning(
|
||||
"onnxruntime 没有 CUDA provider，%s 会跑在 CPU 上（慢）。"
|
||||
"真机请用同名的 .engine",
|
||||
onnx_path.name,
|
||||
)
|
||||
chosen = ["CPUExecutionProvider"]
|
||||
self._sess = ort.InferenceSession(str(onnx_path), providers=chosen)
|
||||
self.in_name = self._sess.get_inputs()[0].name
|
||||
self.out_shape = tuple(self._sess.get_outputs()[0].shape)
|
||||
|
||||
def run(self, chw: np.ndarray) -> np.ndarray:
|
||||
return self._sess.run(None, {self.in_name: chw[None].astype(np.float32)})[0]
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 检测器
|
||||
# --------------------------------------------------------------------------- #
|
||||
class PoseDetector:
|
||||
"""把 pose 模型包装成和 ``YoloDetector`` 一样的 ``detect()/infer()`` 接口。
|
||||
|
||||
多出来的一件事：每个 ``Detection`` 带上 ``kpts``（``(K, 3)`` 的
|
||||
``x, y, conf``，原图像素坐标）。
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
conf: float = 0.25,
|
||||
iou: float = 0.45,
|
||||
imgsz: int = 640,
|
||||
device: str = "0",
|
||||
class_ids: Optional[Sequence[int]] = None,
|
||||
class_names: Optional[Sequence[str]] = None,
|
||||
max_det: int = 20,
|
||||
) -> None:
|
||||
path = Path(weights).expanduser()
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"找不到 pose 权重：{path}")
|
||||
|
||||
info = read_pose_info(path) or {}
|
||||
self.kpt_count = int(info.get("kpt_count") or 0)
|
||||
if self.kpt_count <= 0:
|
||||
raise RuntimeError(f"{path.name} 不是可识别的 pose 模型（缺 kpt_shape）")
|
||||
self.weights_path = str(path)
|
||||
self.conf = float(conf)
|
||||
self.iou = float(iou)
|
||||
self.imgsz = int(imgsz)
|
||||
self.max_det = int(max_det)
|
||||
|
||||
if path.suffix.lower() in {".engine", ".trt"}:
|
||||
self._runner = _TrtRunner(path, device)
|
||||
self.backend = "tensorrt"
|
||||
else:
|
||||
self._runner = _OnnxRunner(path)
|
||||
self.backend = "onnx"
|
||||
|
||||
# 类别标签仍然走统一入口（旁挂 .labels.yaml 优先）。
|
||||
# engine 没有 metadata，所以把 ONNX 里的 names 当"模型内嵌"传进去。
|
||||
names = info.get("names") or {}
|
||||
|
||||
class _Meta: # 只为 resolve_class_labels 能读到 ONNX 内嵌标签
|
||||
pass
|
||||
|
||||
meta = _Meta()
|
||||
meta.names = names
|
||||
meta.nc = int(info.get("nc") or 0)
|
||||
self._labels = resolve_class_labels(path, meta)
|
||||
|
||||
self.class_ids = set(int(v) for v in class_ids) if class_ids else None
|
||||
self.class_names = (
|
||||
{str(n).strip().lower() for n in class_names if str(n).strip()}
|
||||
if class_names
|
||||
else None
|
||||
)
|
||||
self._warmed = False
|
||||
|
||||
# -- 对外 ---------------------------------------------------------------
|
||||
def warmup(self, *, half: bool = False) -> None:
|
||||
del half # engine 已是 FP16；ONNX 这边没有 half 的开关
|
||||
if self._warmed:
|
||||
return
|
||||
# 先置位：infer() 开头会检查 _warmed，不置位就会反过来再调 warmup。
|
||||
self._warmed = True
|
||||
try:
|
||||
self.infer(np.zeros((self.imgsz, self.imgsz, 3), dtype=np.uint8))
|
||||
except Exception:
|
||||
self._warmed = False
|
||||
raise
|
||||
|
||||
def detect(
|
||||
self,
|
||||
image_bgr: np.ndarray,
|
||||
conf: float | None = None,
|
||||
iou: float | None = None,
|
||||
imgsz: int | None = None,
|
||||
**_kwargs,
|
||||
) -> List[Detection]:
|
||||
"""``detect_holes`` 用的别名接口（xyxy + conf + kpts）。"""
|
||||
if conf is not None:
|
||||
self.conf = float(conf)
|
||||
if iou is not None:
|
||||
self.iou = float(iou)
|
||||
# imgsz 在这里**不能**改：engine 的输入尺寸是编译期定死的。
|
||||
if imgsz is not None and int(imgsz) != self.imgsz:
|
||||
logger.warning(
|
||||
"pose engine 输入固定 %d，忽略调用方的 imgsz=%s", self.imgsz, imgsz
|
||||
)
|
||||
return self.infer(image_bgr)
|
||||
|
||||
def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0) -> List[Detection]:
|
||||
del half, seq
|
||||
if image_bgr is None or getattr(image_bgr, "size", 0) == 0:
|
||||
return []
|
||||
if not self._warmed:
|
||||
self.warmup()
|
||||
padded, gain, (dw, dh) = letterbox(image_bgr, self.imgsz)
|
||||
chw = np.ascontiguousarray(
|
||||
padded[:, :, ::-1].transpose(2, 0, 1), dtype=np.float32
|
||||
) / 255.0
|
||||
out = self._runner.run(chw)
|
||||
return self._decode(out, gain, dw, dh)
|
||||
|
||||
# -- 解码 ---------------------------------------------------------------
|
||||
def _decode(
|
||||
self, raw: np.ndarray, gain: float, dw: float, dh: float
|
||||
) -> List[Detection]:
|
||||
o = np.asarray(raw, dtype=np.float32)[0] # (C, A)
|
||||
ch, n_anchor = o.shape
|
||||
k = self.kpt_count
|
||||
nc = int(ch - 4 - 3 * k)
|
||||
if nc < 1:
|
||||
raise RuntimeError(
|
||||
f"pose 输出通道数不对：C={ch}，按 kpt={k} 反推 nc={nc}。"
|
||||
"引擎/onnx 与 kpt_shape 对不上"
|
||||
)
|
||||
|
||||
scores = o[4 : 4 + nc] # (nc, A)
|
||||
cls_ids = scores.argmax(axis=0) # (A,)
|
||||
confs = scores[cls_ids, np.arange(n_anchor)]
|
||||
|
||||
keep = confs >= self.conf
|
||||
if not np.any(keep):
|
||||
return []
|
||||
xywh = o[0:4, keep].T # (N, 4)
|
||||
confs = confs[keep]
|
||||
cls_ids = cls_ids[keep]
|
||||
kpts = o[4 + nc :, keep].reshape(k, 3, -1).transpose(2, 0, 1) # (N, k, 3)
|
||||
|
||||
xyxy = np.empty_like(xywh)
|
||||
xyxy[:, 0] = xywh[:, 0] - 0.5 * xywh[:, 2]
|
||||
xyxy[:, 1] = xywh[:, 1] - 0.5 * xywh[:, 3]
|
||||
xyxy[:, 2] = xywh[:, 0] + 0.5 * xywh[:, 2]
|
||||
xyxy[:, 3] = xywh[:, 1] + 0.5 * xywh[:, 3]
|
||||
|
||||
keep_i = nms_xyxy(xyxy, confs, self.iou, self.max_det)
|
||||
if keep_i.size == 0:
|
||||
return []
|
||||
xyxy, confs, cls_ids, kpts = xyxy[keep_i], confs[keep_i], cls_ids[keep_i], kpts[keep_i]
|
||||
|
||||
# 去 letterbox 的填充，再还原到原图尺度
|
||||
xyxy = xyxy.astype(float)
|
||||
xyxy[:, [0, 2]] = (xyxy[:, [0, 2]] - dw) / gain
|
||||
xyxy[:, [1, 3]] = (xyxy[:, [1, 3]] - dh) / gain
|
||||
kpts = kpts.astype(float)
|
||||
kpts[:, :, 0] = (kpts[:, :, 0] - dw) / gain
|
||||
kpts[:, :, 1] = (kpts[:, :, 1] - dh) / gain
|
||||
|
||||
out: List[Detection] = []
|
||||
for i in range(len(xyxy)):
|
||||
cid = int(cls_ids[i])
|
||||
name = self._labels.get(cid)
|
||||
if self.class_ids is not None and cid not in self.class_ids:
|
||||
continue
|
||||
if self.class_names is not None and name.lower() not in self.class_names:
|
||||
continue
|
||||
out.append(
|
||||
Detection(
|
||||
xyxy=xyxy[i],
|
||||
conf=float(confs[i]),
|
||||
cls_id=cid,
|
||||
cls_name=name,
|
||||
kpts=kpts[i].copy(),
|
||||
)
|
||||
)
|
||||
out.sort(key=lambda d: d.conf, reverse=True)
|
||||
return out
|
||||
|
||||
def close(self) -> None:
|
||||
return None
|
||||
|
||||
|
||||
def keypoint_roles(kpt_count: int) -> Tuple[int, Tuple[int, ...]]:
|
||||
"""返回 ``(跟踪点下标, 孔下标元组)``，供调用方统一口径。
|
||||
|
||||
现场约定：k0 跟踪，k1/k2 对孔。模型关键点少于 3 个时只返回存在的部分。
|
||||
"""
|
||||
track = TRACK_KPT_INDEX if kpt_count > TRACK_KPT_INDEX else -1
|
||||
holes = tuple(i for i in HOLE_KPT_INDICES if i < kpt_count)
|
||||
return track, holes
|
||||
@@ -0,0 +1,57 @@
|
||||
"""Stop an owned subprocess tree even when descendants create separate sessions."""
|
||||
from __future__ import annotations
|
||||
import os
|
||||
import signal
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def _stat(pid):
|
||||
try:
|
||||
fields=(Path('/proc')/str(pid)/'stat').read_text().rsplit(')',1)[1].split()
|
||||
return int(fields[1]),int(fields[19]),fields[0]
|
||||
except (OSError,ValueError,IndexError): return None
|
||||
|
||||
|
||||
def _descendants(pid):
|
||||
table={}
|
||||
for path in Path('/proc').glob('[0-9]*'):
|
||||
state=_stat(int(path.name))
|
||||
if state: table[int(path.name)]=state
|
||||
owned={pid};changed=True
|
||||
while changed:
|
||||
found={p for p,state in table.items() if state[0] in owned}-owned
|
||||
changed=bool(found);owned.update(found)
|
||||
return {p:table[p][1] for p in owned if p in table}
|
||||
|
||||
|
||||
def _alive(pid, started):
|
||||
state=_stat(pid)
|
||||
return bool(state and state[1]==started and state[2]!='Z')
|
||||
|
||||
|
||||
def stop_process_tree(proc, interrupt_timeout=4., terminate_timeout=2., kill_timeout=1.):
|
||||
"""Signal only recorded PID/start-time identities; never match process names."""
|
||||
if proc.poll() is not None:return
|
||||
owned=_descendants(proc.pid)
|
||||
for sig,timeout in ((signal.SIGINT,interrupt_timeout),(signal.SIGTERM,terminate_timeout),(signal.SIGKILL,kill_timeout)):
|
||||
if proc.poll() is None:owned.update(_descendants(proc.pid))
|
||||
def send(pids):
|
||||
# Root first so the launch system can also run its shutdown handlers.
|
||||
for pid in sorted(pids,key=lambda p:p!=proc.pid):
|
||||
if _alive(pid,owned[pid]):
|
||||
try:os.kill(pid,sig)
|
||||
except ProcessLookupError:pass
|
||||
send(owned)
|
||||
deadline=time.monotonic()+timeout
|
||||
while time.monotonic()<deadline:
|
||||
if proc.poll() is None:
|
||||
new=_descendants(proc.pid);fresh=set(new)-set(owned)
|
||||
owned.update(new);send(fresh)
|
||||
if not any(_alive(pid,started) for pid,started in owned.items()):
|
||||
proc.wait(timeout=1)
|
||||
return
|
||||
time.sleep(.04)
|
||||
living=[pid for pid,started in owned.items() if _alive(pid,started)]
|
||||
if living:raise RuntimeError(f'受管子进程未退出：{living}')
|
||||
proc.wait(timeout=1)
|
||||
@@ -0,0 +1,977 @@
|
||||
#!/usr/bin/env python3
|
||||
"""把 RealSense 彩色流发布到 ROS 话题（不依赖 realsense2_camera 的 apt 包）。
|
||||
|
||||
当你没装 ``ros-humble-realsense2-camera``、但 ``pyrealsense2`` 能正常工作时用它。
|
||||
|
||||
默认话题（camera_name=d405）：
|
||||
/d405/color/image_raw
|
||||
/d405/color/camera_info
|
||||
/d405/aligned_depth_to_color/image_raw (16UC1 mm，需开启 enable_depth)
|
||||
|
||||
性能说明
|
||||
--------
|
||||
* 在采集线程里直接发布（没有定时器带来的延迟）。
|
||||
* 使用 SensorDataQoS（BEST_EFFORT，depth=1），这样慢的订阅者会主动丢旧帧。
|
||||
* 可选的 SDK 预览：``-p show_preview:=true``（沿用 Intel 示例的链路：pipeline → numpy → 窗口）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import array
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
|
||||
# RealSense 掉到 480Mbps（或整台消失）时的排查口径。写在这里是为了"只有一处"：
|
||||
# 相机节点的报错、preflight 脚本、文档都引用同一套说法，不会各说各的。
|
||||
# 为什么 480Mbps 值得专门解释一次（2026-09-28 现场）：
|
||||
# D435 原本协商到 USB 3.2、1280x720@30 跑得好好的，中途
|
||||
# `xioctl(UVCIOC_CTRL_QUERY) ... No such file or directory`（= 设备从总线上消失）
|
||||
# 之后重新枚举成了 **USB 2.1**。USB3 线缆/座子里 SuperSpeed 那一对针脚一旦
|
||||
# 接触不良，USB2 的针脚还能用 —— 于是设备不会"掉线"，只是**悄悄掉速**，
|
||||
# 表现成"相机在，但帧率/分辨率不对"。这时查软件是查不出来的。
|
||||
USB_RESEAT_HINT = (
|
||||
'拔插一次、换一根 USB3 线，或换到机箱后面板直连的蓝色口 —— '
|
||||
'只协商到 480Mbps 通常是 SuperSpeed 针脚没接上，不是相机坏了。'
|
||||
'看当前协商速率：lsusb -t（设备后面写 480M 还是 5000M）。'
|
||||
)
|
||||
|
||||
|
||||
def _fill_camera_info(
|
||||
msg: CameraInfo, width: int, height: int, fx, fy, cx, cy, frame_id: str, dist
|
||||
) -> None:
|
||||
msg.width = width
|
||||
msg.height = height
|
||||
msg.distortion_model = 'plumb_bob'
|
||||
msg.d = [float(v) for v in list(dist)[:5]]
|
||||
while len(msg.d) < 5:
|
||||
msg.d.append(0.0)
|
||||
msg.k = [
|
||||
float(fx), 0.0, float(cx),
|
||||
0.0, float(fy), float(cy),
|
||||
0.0, 0.0, 1.0,
|
||||
]
|
||||
msg.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
|
||||
msg.p = [
|
||||
float(fx), 0.0, float(cx), 0.0,
|
||||
0.0, float(fy), float(cy), 0.0,
|
||||
0.0, 0.0, 1.0, 0.0,
|
||||
]
|
||||
msg.header.frame_id = frame_id
|
||||
|
||||
|
||||
def _site_yaml_path() -> Path:
|
||||
from arm_application.site_io import site_path
|
||||
|
||||
return site_path()
|
||||
|
||||
|
||||
class RealsenseColorPub(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('realsense_color_pub')
|
||||
self.declare_parameter('camera_name', 'd405') # 话题命名空间
|
||||
self._name = str(self.get_parameter('camera_name').value).strip().strip('/')
|
||||
self._serial = param_as_str(self, 'camera_serial', '')
|
||||
self.declare_parameter('width', 1280)
|
||||
self.declare_parameter('height', 720)
|
||||
self.declare_parameter('fps', 15)
|
||||
self.declare_parameter('show_preview', False)
|
||||
self.declare_parameter('overlay_detection', True)
|
||||
self.declare_parameter('overlay_hold_sec', 1.5)
|
||||
self.declare_parameter('enable_depth', True)
|
||||
self.declare_parameter('enable_infra', True)
|
||||
self.declare_parameter('preview_stereo', True)
|
||||
# Qt 预览每 N 帧才渲染一次 —— 发布始终保持满帧率；
|
||||
# CPU 上色 + 四窗格绘制绝不能拖慢数据流。
|
||||
self.declare_parameter('preview_decimation', 2)
|
||||
self.declare_parameter('auto_exposure', True)
|
||||
self.declare_parameter('ae_limit_us', 3000.0)
|
||||
self.declare_parameter('emitter_enabled', False)
|
||||
self.declare_parameter('laser_power', 0.0)
|
||||
self.declare_parameter('exposure_us', 0.0)
|
||||
self.declare_parameter('gain', 0.0)
|
||||
|
||||
self._width = int(self.get_parameter('width').value)
|
||||
self._height = int(self.get_parameter('height').value)
|
||||
self._fps = int(self.get_parameter('fps').value)
|
||||
self._show_preview = bool(self.get_parameter('show_preview').value)
|
||||
self._overlay_detection = bool(self.get_parameter('overlay_detection').value)
|
||||
self._overlay_hold_sec = float(self.get_parameter('overlay_hold_sec').value)
|
||||
self._enable_depth_param = bool(self.get_parameter('enable_depth').value)
|
||||
self._enable_infra_param = bool(self.get_parameter('enable_infra').value)
|
||||
self._preview_stereo = bool(self.get_parameter('preview_stereo').value)
|
||||
self._preview_every = max(
|
||||
1, int(self.get_parameter('preview_decimation').value)
|
||||
)
|
||||
self._auto_exposure = bool(self.get_parameter('auto_exposure').value)
|
||||
self._ae_limit_us = float(self.get_parameter('ae_limit_us').value)
|
||||
self._emitter_enabled = bool(self.get_parameter('emitter_enabled').value)
|
||||
self._laser_power = float(self.get_parameter('laser_power').value)
|
||||
self._exposure_us = float(self.get_parameter('exposure_us').value)
|
||||
self._gain = float(self.get_parameter('gain').value)
|
||||
# FPS 表：硬件帧号对比实际发布帧数（循环速率）
|
||||
self._fps_t0 = 0.0
|
||||
self._fps_pub_color = 0
|
||||
self._fps_pub_depth = 0
|
||||
self._fps_fn_color = -1
|
||||
self._fps_fn_depth = -1
|
||||
self._t_wait = 0.0
|
||||
self._t_align = 0.0
|
||||
self._t_pub = 0.0
|
||||
|
||||
qos = qos_profile_sensor_data # BEST_EFFORT，只保留最后 1 帧
|
||||
base = f'/{self._name}/color'
|
||||
self._img_pub = self.create_publisher(
|
||||
Image, f'{base}/image_raw', qos
|
||||
)
|
||||
self._info_pub = self.create_publisher(
|
||||
CameraInfo, f'{base}/camera_info', qos
|
||||
)
|
||||
self._depth_pub = self.create_publisher(
|
||||
Image, f'/{self._name}/aligned_depth_to_color/image_raw', qos
|
||||
)
|
||||
# IR 流也会一起发布，这样预览可以放在独立的查看器进程里
|
||||
# （Qt 不能和采集循环共享 GIL）。
|
||||
self._ir1_pub = None
|
||||
self._ir2_pub = None
|
||||
if self._enable_infra_param and self._preview_stereo:
|
||||
self._ir1_pub = self.create_publisher(
|
||||
Image, f'/{self._name}/infra1/image_rect_raw', qos
|
||||
)
|
||||
self._ir2_pub = self.create_publisher(
|
||||
Image, f'/{self._name}/infra2/image_rect_raw', qos
|
||||
)
|
||||
|
||||
self._pipe = None
|
||||
self._profile = None
|
||||
self._align = None
|
||||
self._have_depth = False
|
||||
self._have_infra = False
|
||||
self._infra_mode = 'none'
|
||||
self._colorizer = None
|
||||
self._logged_depth = False
|
||||
# 设备深度尺度 → 用原始 uint16 乘上它才是真实的 mm。
|
||||
# D405 = 0.1 mm/单位，D435 = 1.0 mm/单位。启动时从传感器读取。
|
||||
self._depth_to_mm = 1.0
|
||||
self._running = True
|
||||
self._thread: Optional[threading.Thread] = None
|
||||
self._fx = self._fy = self._cx = self._cy = 0.0
|
||||
self._dist = [0.0] * 5
|
||||
self._frame_id = f'{self._name}_color_optical_frame'
|
||||
self._info_every = 1
|
||||
self._frame_i = 0
|
||||
self._preview = None
|
||||
self._overlay: Optional[list] = None
|
||||
self._overlay_stamp = 0.0
|
||||
self._capture_started = False
|
||||
self._cap_lock = threading.Lock()
|
||||
self._cap_event = threading.Event()
|
||||
self._pending = None
|
||||
self._last_frame_mono = 0.0
|
||||
self._usb_desc = ''
|
||||
self._last_ok = None
|
||||
self._site_yaml = _site_yaml_path()
|
||||
self._site_mtime = 0.0
|
||||
self._site_wh_warned = False
|
||||
if self._show_preview:
|
||||
from arm_application.realsense_sdk_view import SdkPreviewWindow, ensure_app
|
||||
|
||||
self._qt_app = ensure_app()
|
||||
self._preview = SdkPreviewWindow(
|
||||
title=f'RealSense {self._name} COLOR|DEPTH|IR-L|IR-R (pyrealsense2 SDK)'
|
||||
)
|
||||
self._preview.show()
|
||||
if self._overlay_detection:
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
|
||||
det_qos = QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
)
|
||||
self.create_subscription(
|
||||
Float32MultiArray,
|
||||
'workpiece/detection',
|
||||
self._on_det,
|
||||
det_qos,
|
||||
)
|
||||
self.get_logger().info(
|
||||
'SDK 预览已开启 —— 本进程内显示 COLOR|DEPTH|IR-L|IR-R '
|
||||
f'（保留 {self._overlay_hold_sec:.1f}s）。关窗口 ≠ 停相机。'
|
||||
)
|
||||
self._start_pipeline()
|
||||
|
||||
def _pick_device(self, devices, rs):
|
||||
"""优先用配置的序列号，其次按名字选 D405（这台机器上还接了 D435）。"""
|
||||
hint = (self._name or 'd405').lower()
|
||||
if self._serial:
|
||||
for d in devices:
|
||||
if d.get_info(rs.camera_info.serial_number) == self._serial:
|
||||
return d
|
||||
avail = ', '.join(
|
||||
f"{d.get_info(rs.camera_info.name)}:{d.get_info(rs.camera_info.serial_number)}"
|
||||
for d in devices
|
||||
)
|
||||
raise RuntimeError(
|
||||
f'找不到序列号 {self._serial!r}。当前已连接：{avail}'
|
||||
)
|
||||
for d in devices:
|
||||
name = str(d.get_info(rs.camera_info.name) or '').lower()
|
||||
if hint in name or (hint == 'd405' and '405' in name):
|
||||
return d
|
||||
return devices[0]
|
||||
|
||||
def _reload_site_preview(self, stream_w: int = 0, stream_h: int = 0) -> None:
|
||||
"""不重启就能读到新的 overlay_hold_sec（宽高变了则给出警告）。"""
|
||||
path = self._site_yaml
|
||||
if not path.is_file():
|
||||
return
|
||||
try:
|
||||
mtime = path.stat().st_mtime
|
||||
except OSError:
|
||||
return
|
||||
if mtime <= self._site_mtime:
|
||||
return
|
||||
self._site_mtime = mtime
|
||||
try:
|
||||
import yaml
|
||||
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
cam = data.get("camera") or {}
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f"site.yaml 重新加载失败：{exc}")
|
||||
return
|
||||
if "overlay_hold_sec" in cam:
|
||||
hold = max(0.0, float(cam.get("overlay_hold_sec") or 0.0))
|
||||
if abs(hold - self._overlay_hold_sec) > 1e-6:
|
||||
self.get_logger().info(
|
||||
f"overlay_hold_sec {self._overlay_hold_sec:.2f}s → {hold:.2f}s (site.yaml)"
|
||||
)
|
||||
self._overlay_hold_sec = hold
|
||||
want_w = int(cam.get("width") or self._width)
|
||||
want_h = int(cam.get("height") or self._height)
|
||||
live_w = stream_w or self._width
|
||||
live_h = stream_h or self._height
|
||||
if (want_w, want_h) != (live_w, live_h) and not self._site_wh_warned:
|
||||
self._site_wh_warned = True
|
||||
self.get_logger().warn(
|
||||
f"预览流是 {live_w}x{live_h}，而 site.yaml 里的 camera 是 "
|
||||
f"{want_w}x{want_h} —— 请重启：bash ~/ros2/scripts/run_camera_d405.sh --preview"
|
||||
)
|
||||
elif (want_w, want_h) == (live_w, live_h):
|
||||
self._site_wh_warned = False
|
||||
|
||||
def _on_det(self, msg: Float32MultiArray) -> None:
|
||||
# 空负载 = 感知漏检 → 立刻清掉绿框
|
||||
if not msg.data or len(msg.data) < 8:
|
||||
self._overlay = None
|
||||
self._overlay_stamp = 0.0
|
||||
if self._preview is not None:
|
||||
self._preview.set_overlay(None)
|
||||
return
|
||||
self._overlay = list(msg.data)
|
||||
self._overlay_stamp = time.monotonic()
|
||||
if self._preview is not None:
|
||||
self._preview.set_overlay(self._overlay)
|
||||
|
||||
def _publish_frame(self, bgr: np.ndarray, depth_u16: Optional[np.ndarray] = None) -> None:
|
||||
h, w = bgr.shape[0], bgr.shape[1]
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
|
||||
img = Image()
|
||||
img.header.stamp = stamp
|
||||
img.header.frame_id = self._frame_id
|
||||
img.height = h
|
||||
img.width = w
|
||||
img.encoding = 'bgr8'
|
||||
img.is_bigendian = 0
|
||||
img.step = w * 3
|
||||
# array.array('B', ...) 会走 setter 的快路径 —— 直接传 bytes 会触发
|
||||
# 逐元素的 debug 检查，在这里每帧要多花约 40 ms。
|
||||
img.data = array.array('B', np.ascontiguousarray(bgr).tobytes())
|
||||
self._img_pub.publish(img)
|
||||
|
||||
info = CameraInfo()
|
||||
info.header.stamp = stamp
|
||||
info.header.frame_id = self._frame_id
|
||||
_fill_camera_info(
|
||||
info, w, h, self._fx, self._fy, self._cx, self._cy,
|
||||
self._frame_id, self._dist,
|
||||
)
|
||||
self._info_pub.publish(info)
|
||||
|
||||
if depth_u16 is not None:
|
||||
self._publish_depth(depth_u16, stamp)
|
||||
|
||||
def _publish_depth(self, depth_u16: np.ndarray, stamp=None) -> None:
|
||||
dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
|
||||
dmsg = Image()
|
||||
dmsg.header.stamp = stamp or self.get_clock().now().to_msg()
|
||||
dmsg.header.frame_id = self._frame_id
|
||||
dmsg.height = dh
|
||||
dmsg.width = dw
|
||||
dmsg.encoding = '16UC1'
|
||||
dmsg.is_bigendian = 0
|
||||
dmsg.step = dw * 2
|
||||
dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
|
||||
self._depth_pub.publish(dmsg)
|
||||
if not self._logged_depth:
|
||||
self._logged_depth = True
|
||||
self.get_logger().info(
|
||||
f'首个对齐深度 {dw}x{dh} → '
|
||||
f'/{self._name}/aligned_depth_to_color/image_raw'
|
||||
)
|
||||
|
||||
def _start_pipeline(self, reconnect: bool = False) -> None:
|
||||
try:
|
||||
import pyrealsense2 as rs
|
||||
except Exception as exc: # noqa: BLE001
|
||||
raise RuntimeError(
|
||||
'pyrealsense2 不可用。请执行 conda activate ros_flexiv'
|
||||
) from exc
|
||||
|
||||
# 杀掉上一个发布进程后，USB 需要 1–3s 才能重新枚举。
|
||||
ctx = rs.context()
|
||||
devices = []
|
||||
for attempt in range(8):
|
||||
devices = list(ctx.devices)
|
||||
if devices:
|
||||
break
|
||||
time.sleep(0.4)
|
||||
ctx = rs.context()
|
||||
if not devices:
|
||||
raise RuntimeError(
|
||||
'没有连接 RealSense（多次重试后 USB 仍为空）。'
|
||||
'拔插一次；确认没有 realsense-viewer / 其它进程占着这台相机；'
|
||||
+ USB_RESEAT_HINT
|
||||
)
|
||||
|
||||
found = self._pick_device(devices, rs)
|
||||
try:
|
||||
self._serial = str(found.get_info(rs.camera_info.serial_number))
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
usb_desc = ''
|
||||
try:
|
||||
usb_desc = str(found.get_info(rs.camera_info.usb_type_descriptor))
|
||||
except Exception: # noqa: BLE001
|
||||
usb_desc = '?'
|
||||
self._usb_desc = usb_desc
|
||||
usb2 = usb_desc.startswith('2')
|
||||
cam_name = ''
|
||||
try:
|
||||
cam_name = str(found.get_info(rs.camera_info.name))
|
||||
except Exception: # noqa: BLE001
|
||||
cam_name = self._name
|
||||
self.get_logger().info(
|
||||
f'{cam_name} serial={self._serial or "?"} USB={usb_desc} '
|
||||
f'requested {self._width}x{self._height}@{self._fps}Hz'
|
||||
)
|
||||
if usb2:
|
||||
# 只在 480Mbps 口上才降。实测（2026-09-28）640x480@15 彩色+深度
|
||||
# 可以稳定跑满 15fps；但 1280x720@30 在 USB2 上物理不可能
|
||||
# （SuperSpeed 没协商上）。
|
||||
self.get_logger().warn(
|
||||
f'{cam_name} 只协商到 USB {usb_desc}（480Mbps）—— 降到 '
|
||||
f'640x480@15Hz。' + USB_RESEAT_HINT
|
||||
)
|
||||
self._width, self._height, self._fps = 640, 480, 15
|
||||
|
||||
want = (self._width, self._height, self._fps)
|
||||
last = self._last_ok
|
||||
if reconnect:
|
||||
candidates = []
|
||||
if last:
|
||||
candidates.append(last)
|
||||
if want not in candidates:
|
||||
candidates.append(want)
|
||||
if (640, 480, 15) not in candidates:
|
||||
candidates.append((640, 480, 15))
|
||||
else:
|
||||
candidates = [
|
||||
want,
|
||||
(1280, 720, 15),
|
||||
(640, 480, 15),
|
||||
(640, 480, 5),
|
||||
]
|
||||
if usb2:
|
||||
candidates = [(640, 480, 15), (640, 480, 5), (424, 240, 15)]
|
||||
# 丢弃固件里的垃圾模式（848@90 之类），它们会 ioctl 失败并卡死 USB。
|
||||
seen = set()
|
||||
trimmed = []
|
||||
for item in candidates:
|
||||
if item in seen:
|
||||
continue
|
||||
w, h, fps = item
|
||||
if fps not in (5, 15, 30):
|
||||
continue
|
||||
if w * h > 1280 * 720:
|
||||
continue
|
||||
seen.add(item)
|
||||
trimmed.append(item)
|
||||
candidates = trimmed or [(640, 480, 15)]
|
||||
|
||||
pipe = rs.pipeline()
|
||||
profile = None
|
||||
last_err = None
|
||||
want_depth = bool(self._enable_depth_param)
|
||||
want_infra = (
|
||||
bool(self._enable_infra_param)
|
||||
and bool(self._preview_stereo)
|
||||
)
|
||||
have_depth = False
|
||||
have_infra = False
|
||||
|
||||
# USB3：先尝试真正的 IR-L+IR-R（共享立体自动曝光）。彩色+IR-L 在 D405 上
|
||||
# 经常失败 —— 那就退回 IR-R + 把彩色当左目（观感不同，且不共享自动曝光）。
|
||||
def _modes():
|
||||
if want_depth and want_infra and not usb2:
|
||||
yield True, 'both'
|
||||
if want_depth and want_infra:
|
||||
yield True, 'right'
|
||||
if want_depth:
|
||||
yield True, 'none'
|
||||
yield False, 'none'
|
||||
|
||||
for w, h, fps in candidates:
|
||||
for with_depth, infra_mode in _modes():
|
||||
try:
|
||||
trial = rs.config()
|
||||
if self._serial:
|
||||
trial.enable_device(self._serial)
|
||||
trial.enable_stream(rs.stream.color, w, h, rs.format.bgr8, fps)
|
||||
if with_depth:
|
||||
trial.enable_stream(
|
||||
rs.stream.depth, w, h, rs.format.z16, fps
|
||||
)
|
||||
if infra_mode in ('both', 'left'):
|
||||
trial.enable_stream(
|
||||
rs.stream.infrared, 1, w, h, rs.format.y8, fps
|
||||
)
|
||||
if infra_mode in ('both', 'right'):
|
||||
trial.enable_stream(
|
||||
rs.stream.infrared, 2, w, h, rs.format.y8, fps
|
||||
)
|
||||
# 回调里只留最新一帧。wait_for_frames 会等彩色、深度、红外齐套，
|
||||
# 场景一动红外对不齐，彩色帧就在这里被丢掉。
|
||||
profile = pipe.start(trial, self._on_sdk_frame)
|
||||
have_depth = with_depth
|
||||
have_infra = infra_mode != 'none'
|
||||
self._infra_mode = infra_mode
|
||||
if (w, h, fps) != want:
|
||||
self.get_logger().warn(
|
||||
f'请求 {want[0]}x{want[1]}@{want[2]}Hz '
|
||||
f'→ 改用 {w}x{h}@{fps}Hz'
|
||||
)
|
||||
if want_depth and not with_depth:
|
||||
self.get_logger().warn(
|
||||
f'{w}x{h}@{fps}Hz 彩色+深度失败 —— 只用彩色'
|
||||
)
|
||||
elif want_infra and infra_mode == 'none':
|
||||
self.get_logger().warn(
|
||||
f'{w}x{h}@{fps}Hz 立体 IR 失败 —— 改用彩色+深度预览'
|
||||
)
|
||||
elif infra_mode == 'both':
|
||||
self.get_logger().info(
|
||||
f'{w}x{h}@{fps}Hz 立体 IR-L+IR-R（共享自动曝光）+ 深度'
|
||||
)
|
||||
elif infra_mode == 'right':
|
||||
self.get_logger().info(
|
||||
f'{w}x{h}@{fps}Hz IR-L=彩色灰度，IR-R=原始 IR '
|
||||
'（明暗不同 —— 这不是两路自动曝光）'
|
||||
)
|
||||
self._width, self._height, self._fps = w, h, fps
|
||||
self._last_ok = (w, h, fps)
|
||||
break
|
||||
except Exception as exc: # noqa: BLE001
|
||||
last_err = exc
|
||||
kind = f'color+depth+IR-{infra_mode}' if with_depth else 'color'
|
||||
self.get_logger().warn(f'{w}x{h}@{fps}Hz {kind} 失败：{exc}')
|
||||
profile = None
|
||||
have_depth = False
|
||||
have_infra = False
|
||||
try:
|
||||
pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
time.sleep(0.8)
|
||||
pipe = rs.pipeline()
|
||||
if profile is not None:
|
||||
break
|
||||
if profile is None:
|
||||
raise RuntimeError(
|
||||
f'{self._name} 没有可用的彩色配置。最后的错误：{last_err}'
|
||||
)
|
||||
|
||||
self._pipe = pipe
|
||||
self._profile = profile
|
||||
try:
|
||||
sensor = profile.get_device().first_color_sensor()
|
||||
if sensor.supports(rs.option.frames_queue_size):
|
||||
# 队列只留最新一张，避免越积越慢，动起来画面还停在旧帧。
|
||||
sensor.set_option(rs.option.frames_queue_size, 1)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
self._have_depth = have_depth
|
||||
self._have_infra = have_infra
|
||||
self._logged_depth = False
|
||||
# 深度按**真实毫米**发布。D405 的原始单位是 0.1 mm（D435 是 1 mm）；
|
||||
# 下游（感知/伺服）都按 mm 处理，所以要按设备尺度归一化。
|
||||
self._depth_to_mm = 1.0
|
||||
if have_depth:
|
||||
try:
|
||||
ds = float(profile.get_device().first_depth_sensor().get_depth_scale())
|
||||
self._depth_to_mm = ds * 1000.0 # m/单位 → mm/单位
|
||||
self.get_logger().info(
|
||||
f'深度尺度 {ds * 1000.0:.4f} mm/单位 → 按真实 mm 发布'
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'读取深度尺度失败，按 mm 处理：{exc}')
|
||||
self._depth_to_mm = 1.0
|
||||
self._align = rs.align(rs.stream.color) if have_depth else None
|
||||
self._colorizer = None
|
||||
if have_depth:
|
||||
try:
|
||||
self._colorizer = rs.colorizer()
|
||||
self._colorizer.set_option(rs.option.color_scheme, 0)
|
||||
except Exception: # noqa: BLE001
|
||||
self._colorizer = None
|
||||
try:
|
||||
from arm_application.handeye_realsense import apply_realsense_exposure
|
||||
import time as _time
|
||||
|
||||
exp_us = self._exposure_us if self._exposure_us > 0 else None
|
||||
gain = self._gain if self._gain > 0 else None
|
||||
kwargs = dict(
|
||||
auto_exposure=self._auto_exposure,
|
||||
ae_limit_us=self._ae_limit_us,
|
||||
emitter_enabled=self._emitter_enabled,
|
||||
laser_power=self._laser_power,
|
||||
exposure_us=exp_us,
|
||||
gain=gain,
|
||||
log=self.get_logger().info,
|
||||
)
|
||||
apply_realsense_exposure(profile.get_device(), **kwargs)
|
||||
# 固件经常会在最初几帧重置自动曝光；所以再设置一次。
|
||||
_time.sleep(0.25)
|
||||
apply_realsense_exposure(profile.get_device(), **kwargs)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'应用曝光参数失败：{exc}')
|
||||
if self._preview is not None:
|
||||
if self._infra_mode == 'both':
|
||||
self._preview.set_pane_titles(
|
||||
'COLOR', 'DEPTH', 'IR-L (左红外，与右目同一曝光)', 'IR-R (右红外)'
|
||||
)
|
||||
elif self._infra_mode == 'right':
|
||||
self._preview.set_pane_titles(
|
||||
'COLOR', 'DEPTH', '左目≈彩图灰度（不是红外曝光）', 'IR-R (右红外)'
|
||||
)
|
||||
else:
|
||||
self._preview.set_pane_titles(
|
||||
'COLOR', 'DEPTH', 'IR-L (无)', 'IR-R (无)'
|
||||
)
|
||||
|
||||
vs = profile.get_stream(rs.stream.color).as_video_stream_profile()
|
||||
try:
|
||||
intr = vs.get_intrinsics()
|
||||
self._fx = float(intr.fx)
|
||||
self._fy = float(intr.fy)
|
||||
self._cx = float(intr.ppx)
|
||||
self._cy = float(intr.ppy)
|
||||
dist = list(intr.coeffs[:5]) if intr.coeffs else [0.0] * 5
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(
|
||||
f'get_intrinsics 失败（{exc}）；改用 D405 的默认内参，'
|
||||
f'分辨率 {self._width}x{self._height}'
|
||||
)
|
||||
w, h = float(self._width), float(self._height)
|
||||
if w >= 1200:
|
||||
self._fx, self._fy = 654.8, 654.0
|
||||
else:
|
||||
self._fx = 393.9 * (w / 640.0)
|
||||
self._fy = 393.4 * (h / 480.0)
|
||||
self._cx, self._cy = w * 0.5, h * 0.5
|
||||
dist = [0.0] * 5
|
||||
while len(dist) < 5:
|
||||
dist.append(0.0)
|
||||
self._dist = dist
|
||||
|
||||
sn = profile.get_device().get_info(rs.camera_info.serial_number)
|
||||
model = profile.get_device().get_info(rs.camera_info.name)
|
||||
depth_txt = (
|
||||
f' + aligned depth /{self._name}/aligned_depth_to_color/image_raw'
|
||||
if have_depth
|
||||
else ' （仅彩色）'
|
||||
)
|
||||
if have_infra:
|
||||
depth_txt += ' + IR-L/IR-R 预览'
|
||||
self.get_logger().info(
|
||||
f'{model} serial={sn} USB={usb_desc} → /{self._name}/color/image_raw '
|
||||
f'{self._width}x{self._height}@{self._fps}Hz '
|
||||
f'fx={self._fx:.1f} fy={self._fy:.1f}{depth_txt} (SensorDataQoS)'
|
||||
)
|
||||
|
||||
if not getattr(self, '_capture_started', False):
|
||||
self._capture_started = True
|
||||
self._thread = threading.Thread(target=self._publish_loop, daemon=True, name="rs-publish")
|
||||
self._thread.start()
|
||||
|
||||
def _enable_color_ae_fast(self, profile, rs) -> None:
|
||||
"""彩色自动曝光，优先用短曝光（减少运动模糊）。"""
|
||||
try:
|
||||
sensor = profile.get_device().first_color_sensor()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'彩色 AE：没有彩色传感器（{exc}）')
|
||||
return
|
||||
try:
|
||||
if sensor.supports(rs.option.enable_auto_exposure):
|
||||
sensor.set_option(rs.option.enable_auto_exposure, 1.0)
|
||||
if sensor.supports(rs.option.auto_exposure_priority):
|
||||
# 0 = 保住帧率，场景够亮时缩短曝光时间
|
||||
sensor.set_option(rs.option.auto_exposure_priority, 0.0)
|
||||
limit = getattr(rs.option, "auto_exposure_limit", None)
|
||||
if limit is not None and sensor.supports(limit):
|
||||
try:
|
||||
sensor.set_option(limit, 5000.0) # µs；30 Hz 下减少运动模糊
|
||||
except Exception:
|
||||
pass
|
||||
self.get_logger().info('彩色自动曝光已开启（优先帧率 / 减少模糊）')
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'彩色 AE：{exc}')
|
||||
|
||||
def _enable_stereo_ae(self, profile, rs) -> None:
|
||||
"""两路 IR 成像器共用一路自动曝光（立体模块）。"""
|
||||
try:
|
||||
sensor = profile.get_device().first_depth_sensor()
|
||||
if sensor.supports(rs.option.enable_auto_exposure):
|
||||
sensor.set_option(rs.option.enable_auto_exposure, 1.0)
|
||||
limit = getattr(rs.option, "auto_exposure_limit", None)
|
||||
if limit is not None and sensor.supports(limit):
|
||||
try:
|
||||
# 把 IR 曝光上限压到远低于 33 ms 的帧周期，这样光线暗的场景
|
||||
# 也不会悄悄把数据流掉到 30 Hz 以下。
|
||||
sensor.set_option(limit, 20000.0)
|
||||
self.get_logger().info('立体 IR 曝光上限 20ms')
|
||||
except Exception:
|
||||
pass
|
||||
self.get_logger().info('立体 IR 自动曝光已开启（左右共享）')
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'立体 AE：{exc}')
|
||||
|
||||
def _colorize_depth(self, depth_frame, depth_u16: np.ndarray) -> np.ndarray:
|
||||
if self._colorizer is not None:
|
||||
try:
|
||||
rgb = np.asanyarray(self._colorizer.colorize(depth_frame).get_data())
|
||||
if rgb.ndim == 3 and rgb.shape[2] >= 3:
|
||||
return rgb[:, :, ::-1].copy()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
d = np.asarray(depth_u16)
|
||||
vis = np.zeros((d.shape[0], d.shape[1], 3), dtype=np.uint8)
|
||||
valid = d > 0
|
||||
if not valid.any():
|
||||
return vis
|
||||
x = d.astype(np.float32)
|
||||
lo, hi = np.percentile(x[valid], [5, 95])
|
||||
hi = max(float(hi), float(lo) + 1.0)
|
||||
gray = np.clip((x - lo) / (hi - lo), 0.0, 1.0)
|
||||
gray = (gray * 255.0).astype(np.uint8)
|
||||
gray[~valid] = 0
|
||||
try:
|
||||
import cv2
|
||||
|
||||
vis = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
|
||||
vis[~valid] = 0
|
||||
except Exception: # noqa: BLE001
|
||||
vis[:, :, 0] = gray
|
||||
vis[:, :, 1] = gray
|
||||
vis[:, :, 2] = gray
|
||||
return vis
|
||||
|
||||
def _reconnect(self) -> bool:
|
||||
try:
|
||||
if self._pipe is not None:
|
||||
self._pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
self._pipe = None
|
||||
time.sleep(2.5)
|
||||
try:
|
||||
self._start_pipeline(reconnect=True)
|
||||
return self._pipe is not None
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(
|
||||
f'重连失败：{exc}。请拔插 USB3，并给线缆留点余量。',
|
||||
throttle_duration_sec=5.0,
|
||||
)
|
||||
return False
|
||||
|
||||
def _on_sdk_frame(self, frame) -> None:
|
||||
"""SDK 回调。只拷走彩色并握住这一组帧，立刻返回。
|
||||
|
||||
对齐、深度换算和 ROS 发布若做在这个回调里，下一帧来不及取，
|
||||
动起来时彩色帧号会跳。那些工作放到 ``_publish_loop``。
|
||||
"""
|
||||
try:
|
||||
frames = frame.as_frameset()
|
||||
color = frames.get_color_frame()
|
||||
if not color:
|
||||
return
|
||||
bgr = np.array(np.asanyarray(color.get_data()), copy=True)
|
||||
frames.keep()
|
||||
with self._cap_lock:
|
||||
self._pending = (bgr, frames)
|
||||
self._last_frame_mono = time.monotonic()
|
||||
self._cap_event.set()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
try:
|
||||
self.get_logger().warn(f'收帧：{exc}', throttle_duration_sec=2.0)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
def _publish_loop(self) -> None:
|
||||
got = False
|
||||
while self._running and rclpy.ok():
|
||||
if self._pipe is None:
|
||||
if not self._reconnect():
|
||||
time.sleep(2.0)
|
||||
continue
|
||||
if not self._cap_event.wait(0.2):
|
||||
silent = self._last_frame_mono and time.monotonic() - self._last_frame_mono > 2.0
|
||||
if silent:
|
||||
usb = getattr(self, '_usb_desc', '?')
|
||||
self.get_logger().error(
|
||||
f'相机超过 2 秒没有新帧（USB={usb}）。正在重连 —— '
|
||||
'请给 USB 线缆留点余量，别让机械臂把它拽掉。'
|
||||
)
|
||||
self._last_frame_mono = 0.0
|
||||
got = False
|
||||
self._reconnect()
|
||||
continue
|
||||
self._cap_event.clear()
|
||||
with self._cap_lock:
|
||||
item = self._pending
|
||||
self._pending = None
|
||||
if item is None:
|
||||
continue
|
||||
bgr, frames = item
|
||||
try:
|
||||
t_pub = time.monotonic()
|
||||
# 彩色先发出去。深度对齐再慢，也不能把这一帧彩色拖掉。
|
||||
self._publish_frame(bgr, None)
|
||||
self._t_pub = time.monotonic() - t_pub
|
||||
ir_l = (
|
||||
frames.get_infrared_frame(1)
|
||||
if self._have_infra and self._infra_mode in ('both', 'left')
|
||||
else None
|
||||
)
|
||||
ir_r = (
|
||||
frames.get_infrared_frame(2)
|
||||
if self._have_infra and self._infra_mode in ('both', 'right')
|
||||
else None
|
||||
)
|
||||
aligned = frames
|
||||
if self._align is not None:
|
||||
t_al = time.monotonic()
|
||||
aligned = self._align.process(frames)
|
||||
self._t_align = time.monotonic() - t_al
|
||||
depth = aligned.get_depth_frame()
|
||||
color = frames.get_color_frame()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
text = str(exc)
|
||||
dropped = (
|
||||
'disconnect' in text.lower()
|
||||
or 'ioctl' in text.lower()
|
||||
or 'errno=5' in text.lower()
|
||||
)
|
||||
if dropped:
|
||||
usb = getattr(self, '_usb_desc', '?')
|
||||
self.get_logger().error(
|
||||
f'相机数据流中断（USB={usb}）：{text[:120]}。'
|
||||
'正在重连 —— 请给 USB 线缆留点余量，别让机械臂把它拽掉。'
|
||||
)
|
||||
got = False
|
||||
self._last_frame_mono = 0.0
|
||||
self._reconnect()
|
||||
else:
|
||||
self.get_logger().warn(f'整理帧：{exc}', throttle_duration_sec=2.0)
|
||||
continue
|
||||
depth_u16 = np.asanyarray(depth.get_data()) if depth else None
|
||||
if depth_u16 is not None and self._depth_to_mm != 1.0:
|
||||
# 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位）
|
||||
depth_u16 = np.clip(
|
||||
depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535
|
||||
).astype(np.uint16)
|
||||
if depth_u16 is not None:
|
||||
try:
|
||||
self._publish_depth(depth_u16)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
if not self._running:
|
||||
break
|
||||
self.get_logger().warn(f'发布深度失败：{exc}', throttle_duration_sec=2.0)
|
||||
self._frame_i += 1
|
||||
self._fps_meter(color, depth, depth_u16)
|
||||
# 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次
|
||||
# —— 它绝不能拖慢发布。
|
||||
do_push = (
|
||||
self._preview is not None
|
||||
and (self._frame_i % self._preview_every) == 0
|
||||
)
|
||||
depth_bgr = None
|
||||
ir_l_img = None
|
||||
ir_r_img = None
|
||||
if do_push:
|
||||
if depth and depth_u16 is not None and depth_u16.size:
|
||||
depth_bgr = self._colorize_depth(depth, depth_u16)
|
||||
ir_l_img = np.asanyarray(ir_l.get_data()).copy() if ir_l else None
|
||||
ir_r_img = np.asanyarray(ir_r.get_data()).copy() if ir_r else None
|
||||
# 只有在拿不到原始左目 IR 流时，才用彩色灰度来顶替。
|
||||
if ir_l_img is None and self._infra_mode != 'both':
|
||||
ir_l_img = np.mean(bgr, axis=2).astype(np.uint8)
|
||||
if not got:
|
||||
got = True
|
||||
sw, sh = int(bgr.shape[1]), int(bgr.shape[0])
|
||||
self.get_logger().info(f'首帧 {sw}x{sh}')
|
||||
if self._preview is not None:
|
||||
self._preview.set_pane_titles(
|
||||
f"COLOR {sw}x{sh}",
|
||||
"DEPTH",
|
||||
"IR-L (左红外)",
|
||||
"IR-R (右红外)",
|
||||
)
|
||||
try:
|
||||
t_pub = time.monotonic()
|
||||
if self._ir1_pub is not None and ir_l is not None:
|
||||
self._publish_ir(ir_l, self._ir1_pub)
|
||||
if self._ir2_pub is not None and ir_r is not None:
|
||||
self._publish_ir(ir_r, self._ir2_pub)
|
||||
self._t_pub = time.monotonic() - t_pub
|
||||
if do_push:
|
||||
self._reload_site_preview(int(bgr.shape[1]), int(bgr.shape[0]))
|
||||
hold = max(0.0, self._overlay_hold_sec)
|
||||
age = time.monotonic() - self._overlay_stamp
|
||||
if self._overlay_detection and self._overlay is not None and age > hold:
|
||||
self._overlay = None
|
||||
self._preview.set_overlay(None)
|
||||
if self._overlay_detection and (
|
||||
self._overlay is None or age > hold
|
||||
):
|
||||
self._preview.set_status(
|
||||
'没有框 —— 检查 perception_node / conf'
|
||||
)
|
||||
else:
|
||||
self._preview.set_status('')
|
||||
self._preview.push_views(
|
||||
bgr.copy(), depth_bgr, ir_l_img, ir_r_img
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
# 上下文可能正在关闭
|
||||
if not self._running:
|
||||
break
|
||||
self.get_logger().warn(
|
||||
f'发布失败：{exc}', throttle_duration_sec=2.0
|
||||
)
|
||||
|
||||
def _publish_ir(self, frame, pub) -> None:
|
||||
"""把原始 IR 成像器以 mono8 发布，供外部查看器进程使用。"""
|
||||
arr = np.asanyarray(frame.get_data())
|
||||
h, w = int(arr.shape[0]), int(arr.shape[1])
|
||||
msg = Image()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = self._frame_id
|
||||
msg.height = h
|
||||
msg.width = w
|
||||
msg.encoding = 'mono8'
|
||||
msg.is_bigendian = 0
|
||||
msg.step = w
|
||||
msg.data = array.array('B', np.ascontiguousarray(arr).tobytes())
|
||||
pub.publish(msg)
|
||||
|
||||
def _fps_meter(self, color, depth, depth_u16) -> None:
|
||||
"""记录发布帧率与硬件帧率 —— 用来区分循环太慢
|
||||
（预览/CPU）和 SDK 侧掉帧（自动曝光长曝光 / USB）。"""
|
||||
now = time.monotonic()
|
||||
try:
|
||||
fn_c = int(color.get_frame_number())
|
||||
except Exception: # noqa: BLE001
|
||||
fn_c = -1
|
||||
try:
|
||||
fn_d = int(depth.get_frame_number()) if depth else -1
|
||||
except Exception: # noqa: BLE001
|
||||
fn_d = -1
|
||||
if self._fps_t0 <= 0.0:
|
||||
self._fps_t0 = now
|
||||
self._fps_pub_color = 0
|
||||
self._fps_pub_depth = 0
|
||||
self._fps_fn_color = fn_c
|
||||
self._fps_fn_depth = fn_d
|
||||
return
|
||||
self._fps_pub_color += 1
|
||||
if depth_u16 is not None:
|
||||
self._fps_pub_depth += 1
|
||||
dt = now - self._fps_t0
|
||||
if dt < 5.0:
|
||||
return
|
||||
hw_c = (fn_c - self._fps_fn_color) / dt if self._fps_fn_color >= 0 and fn_c >= 0 else -1.0
|
||||
hw_d = (fn_d - self._fps_fn_depth) / dt if self._fps_fn_depth >= 0 and fn_d >= 0 else -1.0
|
||||
self.get_logger().info(
|
||||
f'stream fps color={self._fps_pub_color / dt:.1f} (hw {hw_c:.1f}) '
|
||||
f'depth={self._fps_pub_depth / dt:.1f} (hw {hw_d:.1f}) '
|
||||
f'wait={self._t_wait * 1e3:.0f} align={getattr(self, "_t_align", 0.0) * 1e3:.0f} '
|
||||
f'pub={getattr(self, "_t_pub", 0.0) * 1e3:.0f}ms'
|
||||
)
|
||||
self._fps_t0 = now
|
||||
self._fps_pub_color = 0
|
||||
self._fps_pub_depth = 0
|
||||
self._fps_fn_color = fn_c
|
||||
self._fps_fn_depth = fn_d
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = RealsenseColorPub()
|
||||
try:
|
||||
preview = getattr(node, '_preview', None)
|
||||
if preview is not None:
|
||||
app = node._qt_app
|
||||
# 关闭窗口绝不能停掉相机 —— 跟踪需要这条数据流。
|
||||
while rclpy.ok() and node._running:
|
||||
rclpy.spin_once(node, timeout_sec=0.01)
|
||||
app.processEvents()
|
||||
if node._preview is not None and not node._preview.isVisible():
|
||||
node.get_logger().warn(
|
||||
'预览已关闭 —— 相机继续发布'
|
||||
)
|
||||
node._preview = None
|
||||
else:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node._running = False
|
||||
if node._pipe is not None:
|
||||
try:
|
||||
node._pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
if node._thread is not None:
|
||||
node._thread.join(timeout=1.0)
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,236 @@
|
||||
#!/usr/bin/env python3
|
||||
"""RealSense SDK 预览：固定的 2x2 Qt 窗格（彩色 / 深度 / IR-L / IR-R）。
|
||||
|
||||
把四路画面拼成一张 pixmap 时，一旦缺了深度 / IR 看起来就像单相机视图。
|
||||
现在每路画面都有自己带标题的独立窗格。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
from typing import Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
from PySide6.QtCore import Qt, QTimer
|
||||
from PySide6.QtGui import QImage, QPixmap
|
||||
from PySide6.QtWidgets import (
|
||||
QApplication,
|
||||
QGridLayout,
|
||||
QLabel,
|
||||
QMainWindow,
|
||||
QSizePolicy,
|
||||
QVBoxLayout,
|
||||
QWidget,
|
||||
)
|
||||
except Exception: # noqa: BLE001
|
||||
QApplication = None # type: ignore
|
||||
|
||||
|
||||
def _to_bgr(img: Optional[np.ndarray]) -> Optional[np.ndarray]:
|
||||
if img is None:
|
||||
return None
|
||||
vis = img
|
||||
if vis.ndim == 2:
|
||||
vis = np.stack([vis, vis, vis], axis=-1)
|
||||
elif vis.ndim == 3 and vis.shape[2] == 4:
|
||||
vis = vis[:, :, :3]
|
||||
if vis.dtype != np.uint8:
|
||||
vis = np.clip(vis, 0, 255).astype(np.uint8)
|
||||
return np.ascontiguousarray(vis)
|
||||
|
||||
|
||||
def _bgr_to_pixmap(bgr: np.ndarray) -> "QPixmap":
|
||||
rgb = np.ascontiguousarray(bgr[:, :, ::-1])
|
||||
h, w, _ = rgb.shape
|
||||
qimg = QImage(rgb.data, w, h, w * 3, QImage.Format_RGB888).copy()
|
||||
return QPixmap.fromImage(qimg)
|
||||
|
||||
|
||||
def _placeholder(text: str, w: int = 424, h: int = 240) -> np.ndarray:
|
||||
vis = np.zeros((h, w, 3), dtype=np.uint8)
|
||||
vis[:] = (28, 28, 28)
|
||||
try:
|
||||
import cv2
|
||||
|
||||
cv2.putText(
|
||||
vis, text, (16, h // 2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (180, 180, 180), 2
|
||||
)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
return vis
|
||||
|
||||
|
||||
class SdkPreviewWindow(QMainWindow):
|
||||
def __init__(self, title: str = "RealSense") -> None:
|
||||
super().__init__()
|
||||
self.setWindowTitle(title)
|
||||
self._lock = threading.Lock()
|
||||
self._color: Optional[np.ndarray] = None
|
||||
self._depth: Optional[np.ndarray] = None
|
||||
self._ir_l: Optional[np.ndarray] = None
|
||||
self._ir_r: Optional[np.ndarray] = None
|
||||
self._overlay: Optional[Sequence[float]] = None
|
||||
self._status: str = ""
|
||||
self._hud: str = ""
|
||||
|
||||
central = QWidget()
|
||||
grid = QGridLayout(central)
|
||||
grid.setContentsMargins(6, 6, 6, 6)
|
||||
grid.setSpacing(6)
|
||||
self._img_labels: Tuple[QLabel, QLabel, QLabel, QLabel]
|
||||
self._cap_labels: Tuple[QLabel, QLabel, QLabel, QLabel]
|
||||
panes = []
|
||||
caps = []
|
||||
for row, col, name in (
|
||||
(0, 0, "COLOR"),
|
||||
(0, 1, "DEPTH"),
|
||||
(1, 0, "IR-L (左红外)"),
|
||||
(1, 1, "IR-R (右红外)"),
|
||||
):
|
||||
box, lab, cap = self._make_pane(name)
|
||||
grid.addWidget(box, row, col)
|
||||
panes.append(lab)
|
||||
caps.append(cap)
|
||||
self._img_labels = tuple(panes) # type: ignore[assignment]
|
||||
self._cap_labels = tuple(caps) # type: ignore[assignment]
|
||||
self.setCentralWidget(central)
|
||||
self.resize(1360, 820)
|
||||
self.setMinimumSize(880, 520)
|
||||
|
||||
self._timer = QTimer(self)
|
||||
self._timer.timeout.connect(self._redraw)
|
||||
self._timer.start(66)
|
||||
|
||||
@staticmethod
|
||||
def _make_pane(title: str) -> Tuple[QWidget, QLabel, QLabel]:
|
||||
box = QWidget()
|
||||
lay = QVBoxLayout(box)
|
||||
lay.setContentsMargins(0, 0, 0, 0)
|
||||
lay.setSpacing(2)
|
||||
cap = QLabel(title)
|
||||
cap.setAlignment(Qt.AlignCenter)
|
||||
cap.setStyleSheet("color:#e8e8e8; font-weight:600; font-size:13px; background:#222;")
|
||||
img = QLabel("等待画面 …")
|
||||
img.setAlignment(Qt.AlignCenter)
|
||||
img.setMinimumSize(400, 220)
|
||||
img.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
|
||||
img.setStyleSheet("background:#111; color:#777;")
|
||||
img.setScaledContents(False)
|
||||
lay.addWidget(cap)
|
||||
lay.addWidget(img, 1)
|
||||
return box, img, cap
|
||||
|
||||
def set_pane_titles(self, color: str, depth: str, ir_l: str, ir_r: str) -> None:
|
||||
for lab, text in zip(self._cap_labels, (color, depth, ir_l, ir_r)):
|
||||
lab.setText(text)
|
||||
|
||||
def push_bgr(self, bgr: np.ndarray) -> None:
|
||||
self.push_views(bgr)
|
||||
|
||||
def push_views(
|
||||
self,
|
||||
color: np.ndarray,
|
||||
depth_bgr: Optional[np.ndarray] = None,
|
||||
ir_left: Optional[np.ndarray] = None,
|
||||
ir_right: Optional[np.ndarray] = None,
|
||||
) -> None:
|
||||
with self._lock:
|
||||
self._color = color
|
||||
self._depth = depth_bgr
|
||||
self._ir_l = ir_left
|
||||
self._ir_r = ir_right
|
||||
|
||||
def set_overlay(self, data: Optional[Sequence[float]]) -> None:
|
||||
with self._lock:
|
||||
self._overlay = data
|
||||
|
||||
def set_status(self, text: str) -> None:
|
||||
with self._lock:
|
||||
self._status = text or ""
|
||||
|
||||
def _draw_overlay(self, bgr: np.ndarray, ov, status: str) -> np.ndarray:
|
||||
vis = bgr
|
||||
need = (ov is not None and len(ov) >= 8) or bool(status)
|
||||
if not need:
|
||||
return vis
|
||||
try:
|
||||
import cv2
|
||||
except Exception: # noqa: BLE001
|
||||
return vis
|
||||
vis = bgr.copy()
|
||||
if ov is not None and len(ov) >= 8:
|
||||
x1, y1, x2, y2 = [int(round(v)) for v in ov[:4]]
|
||||
cx, cy = int(round(ov[6])), int(round(ov[7]))
|
||||
cv2.rectangle(vis, (x1, y1), (x2, y2), (0, 255, 0), 2)
|
||||
cv2.drawMarker(
|
||||
vis,
|
||||
(vis.shape[1] // 2, vis.shape[0] // 2),
|
||||
(255, 255, 0),
|
||||
cv2.MARKER_CROSS,
|
||||
20,
|
||||
2,
|
||||
)
|
||||
cv2.circle(vis, (cx, cy), 6, (0, 0, 255), -1)
|
||||
conf = float(ov[4]) if len(ov) > 4 else 0.0
|
||||
z = float(ov[8]) if len(ov) > 8 else 0.0
|
||||
cv2.putText(
|
||||
vis,
|
||||
f"{conf:.2f} z={z:.3f}m",
|
||||
(x1, max(20, y1 - 8)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.65,
|
||||
(0, 255, 0),
|
||||
2,
|
||||
)
|
||||
if status:
|
||||
is_right = ("91" in status) or ("右" in status)
|
||||
col = (0, 140, 255) if is_right else (40, 200, 255)
|
||||
cv2.rectangle(vis, (0, 0), (vis.shape[1], 40), (16, 16, 16), -1)
|
||||
cv2.putText(
|
||||
vis,
|
||||
status[:80],
|
||||
(12, 28),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.75,
|
||||
col,
|
||||
2,
|
||||
)
|
||||
return vis
|
||||
|
||||
def _set_pane(self, label: QLabel, bgr: Optional[np.ndarray], empty: str) -> None:
|
||||
vis = _to_bgr(bgr)
|
||||
if vis is None:
|
||||
vis = _placeholder(empty)
|
||||
pix = _bgr_to_pixmap(vis)
|
||||
slot = label.size()
|
||||
if slot.width() > 4 and slot.height() > 4:
|
||||
pix = pix.scaled(slot, Qt.KeepAspectRatio, Qt.FastTransformation)
|
||||
label.setPixmap(pix)
|
||||
|
||||
def _redraw(self) -> None:
|
||||
with self._lock:
|
||||
color = None if self._color is None else self._color
|
||||
depth = self._depth
|
||||
ir_l = self._ir_l
|
||||
ir_r = self._ir_r
|
||||
ov = self._overlay
|
||||
status = self._status
|
||||
if color is None:
|
||||
return
|
||||
color = self._draw_overlay(color, ov, status)
|
||||
labs = self._img_labels
|
||||
self._set_pane(labs[0], color, "无彩色")
|
||||
self._set_pane(labs[1], depth, "无深度")
|
||||
self._set_pane(labs[2], ir_l, "无 IR-L")
|
||||
self._set_pane(labs[3], ir_r, "无 IR-R")
|
||||
|
||||
|
||||
def ensure_app() -> "QApplication":
|
||||
if QApplication is None:
|
||||
raise RuntimeError("RealSense 预览窗口缺少 PySide6，无法创建")
|
||||
app = QApplication.instance()
|
||||
if app is None:
|
||||
app = QApplication([])
|
||||
return app
|
||||
@@ -0,0 +1,118 @@
|
||||
"""应用层节点的统一基类：把「参数类型的宽窄」问题收在一处。
|
||||
|
||||
为什么需要这个基类（2026-09-28 真机落地实测，代价是顶置相机起不来）
|
||||
------------------------------------------------------------------
|
||||
现场 `config/site.yaml` 写的是:
|
||||
|
||||
cameras:
|
||||
d435:
|
||||
ae_limit_us: 8000 # ← YAML 整数
|
||||
|
||||
脚本把它当 `-p ae_limit_us:=8000` 传给节点，而节点里是:
|
||||
|
||||
self.declare_parameter('ae_limit_us', 3000.0) # ← double
|
||||
|
||||
rclpy **严格**比对类型，于是节点**一起就死**:
|
||||
|
||||
InvalidParameterTypeException: Trying to set parameter 'ae_limit_us'
|
||||
to '8000' of type 'INTEGER', expecting type 'DOUBLE'
|
||||
|
||||
现场看到的是「顶置 D435 起不来」，而 site.yaml 那一行看起来毫无问题 ——
|
||||
`8000` 和 `8000.0` 在运维眼里是同一个数，在 rclpy 眼里是两种类型。
|
||||
同一族的雷还有：`width: 848.0`（浮点）喂给 int 参数、`emitter_enabled: 1`
|
||||
喂给 bool 参数、`"26610799"` 与 `26610799`（带不带引号）等等。
|
||||
|
||||
**为什么不让现场人员"写对一点"**：那等于要求每个人记住「这个数必须带小数点」，
|
||||
而记错的表现是节点启动即崩、报错里还完全不提 site.yaml。规则要靠人记住的就
|
||||
一定会破（`FRAMEWORK_RULES.md` 的总纲）。
|
||||
|
||||
**为什么不在每个参数上加个 cast**：异常发生在**声明阶段**，
|
||||
`float(self.get_parameter(...).value)` 根本执行不到 —— 看着宽容，其实没救。
|
||||
|
||||
做法
|
||||
----
|
||||
用 ``ParameterDescriptor(dynamic_typing=True)`` 声明：参数类型由**传进来的值**
|
||||
决定，不做匹配检查；取值时由业务代码 cast（`float(...)` / `int(...)`），
|
||||
语义仍然由声明处的默认值表达。于是 ``8000`` / ``8000.0`` / ``"8000"`` 都能用。
|
||||
|
||||
**代价与边界**：动态类型意味着 rclpy 不再替我们拦「类型写错」。
|
||||
本项目接受这个代价，因为这些参数**全部**来自 `config/site.yaml` 这个运维手改的
|
||||
文件（不是程序内部传参），宽容比严格更符合现场；而"名字写错"仍然会被
|
||||
rclpy 拦下来（参数名是静态的，与类型无关）。
|
||||
|
||||
新增节点一律继承 ``TolerantNode``（有守卫测试
|
||||
``test_ros_node.py::test_no_node_subclasses_rclpy_node_directly`` 钉住）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Optional, Sequence, Union
|
||||
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from rclpy.node import Node
|
||||
|
||||
__all__ = ["TolerantNode", "tolerant_descriptor", "param_as_str"]
|
||||
|
||||
|
||||
def tolerant_descriptor() -> ParameterDescriptor:
|
||||
"""声明用的描述符：类型跟着**传进来的值**走，不做宽窄匹配。"""
|
||||
return ParameterDescriptor(dynamic_typing=True)
|
||||
|
||||
|
||||
def param_as_str(node: Node, name: str, default: str = "") -> str:
|
||||
"""读一个"本质是字符串"的参数，容忍传进来的是数字。
|
||||
|
||||
为什么需要：现场 `site.yaml` 里 `serial_number: 26610799`（不加引号）就是
|
||||
个 YAML 整数，而节点里想要的是字符串。rclpy 同样会因类型不匹配在**声明**
|
||||
阶段抛异常 —— 而字符串参数在运维手上最容易"看着是数字"。
|
||||
|
||||
以前这段逻辑在三个节点里各抄了一份（`dh_pgea_node` / `handeye_teach` /
|
||||
`realsense_color_pub`），相同的东西抄三份迟早歪（`FRAMEWORK_RULES.md` R5.4）。
|
||||
现在只此一处；取不到就返回 `default`。
|
||||
"""
|
||||
node.declare_parameter(name, default, tolerant_descriptor())
|
||||
value = node.get_parameter(name).value
|
||||
if value is None:
|
||||
return default
|
||||
return str(value).strip()
|
||||
|
||||
|
||||
class TolerantNode(Node):
|
||||
"""`rclpy.node.Node` + 「参数类型宽容」。
|
||||
|
||||
只改 `declare_parameter` 一件事：默认描述符换成动态类型。
|
||||
调用方若自己传了 `descriptor`（例如只想放宽某几个），以调用方为准。
|
||||
"""
|
||||
|
||||
def declare_parameter( # type: ignore[override]
|
||||
self,
|
||||
name: str,
|
||||
value: Any = None,
|
||||
descriptor: Optional[ParameterDescriptor] = None,
|
||||
ignore_override: bool = False,
|
||||
) -> Any:
|
||||
if descriptor is None:
|
||||
descriptor = tolerant_descriptor()
|
||||
return super().declare_parameter(name, value, descriptor, ignore_override)
|
||||
|
||||
# 列表版本是 rclpy 的另一个入口，同样要放宽（`workspace_xyz_min: [1, 2, 3]`
|
||||
# 这种"整数字面量的列表"喂给 DOUBLE_ARRAY 一样会崩）。
|
||||
def declare_parameters( # type: ignore[override]
|
||||
self,
|
||||
namespace: str,
|
||||
parameters: Sequence[
|
||||
Union[tuple, Any]
|
||||
],
|
||||
ignore_override: bool = False,
|
||||
) -> Any:
|
||||
fixed = []
|
||||
for item in parameters:
|
||||
if (
|
||||
isinstance(item, tuple)
|
||||
and len(item) == 2
|
||||
and not isinstance(item[1], ParameterDescriptor)
|
||||
):
|
||||
fixed.append((item[0], item[1], tolerant_descriptor()))
|
||||
else:
|
||||
fixed.append(item)
|
||||
return super().declare_parameters(namespace, fixed, ignore_override)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,364 @@
|
||||
"""供应用节点加载 ``~/ros2/config/site.yaml``。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import yaml
|
||||
|
||||
|
||||
def ros2_root() -> Path:
|
||||
"""仓库根目录。先看环境变量，再从本文件向上找 ``config/site.yaml``。
|
||||
|
||||
不能默认 ``~/ros2``：别人把仓库克隆到别的目录时，写死的家目录路径是空的，
|
||||
启动会报文件不存在。
|
||||
"""
|
||||
for key in ("SITE_ROS2_ROOT", "ROS2_WS"):
|
||||
v = os.environ.get(key, "").strip()
|
||||
if v:
|
||||
return Path(v).expanduser()
|
||||
here = Path(__file__).resolve()
|
||||
for parent in here.parents:
|
||||
if (parent / "config" / "site.yaml").is_file() and (parent / "scripts" / "env.sh").is_file():
|
||||
return parent
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
_PATH_KEYS = frozenset({
|
||||
"named_poses_file",
|
||||
"handeye_file",
|
||||
"handeye_waypoints",
|
||||
"handeye_workdir",
|
||||
"handeye_calibrate",
|
||||
"yolo_weights",
|
||||
"hole_weights",
|
||||
"weights",
|
||||
"workpiece_weights",
|
||||
"config_file",
|
||||
"append_poses_file",
|
||||
})
|
||||
|
||||
|
||||
def resolve_repo_path(value: str, root: Path | None = None) -> str:
|
||||
"""仓库内的相对路径展开成绝对路径。已经是绝对路径的原样保留。"""
|
||||
text = str(value or "").strip()
|
||||
if not text:
|
||||
return ""
|
||||
path = Path(text).expanduser()
|
||||
if path.is_absolute():
|
||||
return str(path)
|
||||
base = root or ros2_root()
|
||||
return str((base / path).resolve())
|
||||
|
||||
|
||||
def _is_path_key(key: str) -> bool:
|
||||
return (
|
||||
key in _PATH_KEYS
|
||||
or key.startswith("handeye_file")
|
||||
or key.startswith("handeye_waypoints")
|
||||
or key.endswith("_weights")
|
||||
or key.endswith("_file")
|
||||
or key.endswith("_workdir")
|
||||
)
|
||||
|
||||
|
||||
def absolutize_site_paths(data: Any, root: Path | None = None) -> Any:
|
||||
"""把 site 里的仓库相对路径展开。只动路径字段，不动别的字符串。"""
|
||||
base = root or ros2_root()
|
||||
if isinstance(data, dict):
|
||||
out: dict[Any, Any] = {}
|
||||
for key, val in data.items():
|
||||
name = str(key)
|
||||
if isinstance(val, str) and _is_path_key(name):
|
||||
out[key] = resolve_repo_path(val, base)
|
||||
elif (
|
||||
name == "config"
|
||||
and isinstance(val, str)
|
||||
and (val.endswith((".yaml", ".yml", ".json")) or "/" in val)
|
||||
):
|
||||
out[key] = resolve_repo_path(val, base)
|
||||
else:
|
||||
out[key] = absolutize_site_paths(val, base)
|
||||
return out
|
||||
if isinstance(data, list):
|
||||
return [absolutize_site_paths(item, base) for item in data]
|
||||
return data
|
||||
|
||||
|
||||
def site_path() -> Path:
|
||||
env = os.environ.get("SITE_CONFIG", "").strip()
|
||||
if env:
|
||||
return Path(env).expanduser()
|
||||
return ros2_root() / "config" / "site.yaml"
|
||||
|
||||
|
||||
def arm_short_id(sn: str) -> str:
|
||||
"""序列号末两位就是臂号（``Rizon4-063989`` → ``89``）。
|
||||
|
||||
和 ``scripts/lib/export_site_env.arm_short_id`` 同一条规则。换一台臂
|
||||
只改 ``site.yaml`` 的 SN，不要在界面里再写一张本机对照表。
|
||||
"""
|
||||
tail = str(sn or "").strip().rsplit("-", 1)[-1]
|
||||
return tail[-2:] if tail.isdigit() and len(tail) >= 2 else ""
|
||||
|
||||
|
||||
def listed_arms(data: dict[str, Any] | None) -> list[dict[str, Any]]:
|
||||
"""``robots[]`` 里的真机臂。``sn: sim`` 不进控制。
|
||||
|
||||
旋转爪只看夹爪型号里有没有 ``RGI``。右臂也可以是平行爪，不能靠臂号猜。
|
||||
"""
|
||||
robots = ((data or {}).get("robot") or {}).get("robots") or []
|
||||
out: list[dict[str, Any]] = []
|
||||
seen: set[str] = set()
|
||||
for robot in robots:
|
||||
if not isinstance(robot, dict):
|
||||
continue
|
||||
sn = str(robot.get("sn") or "").strip()
|
||||
if not sn or sn.lower() == "sim":
|
||||
continue
|
||||
arm_no = arm_short_id(sn)
|
||||
if not arm_no:
|
||||
continue
|
||||
if arm_no in seen:
|
||||
raise ValueError(f"两台臂的序列号末两位都是 {arm_no}，--arm 分不清")
|
||||
seen.add(arm_no)
|
||||
side = str(robot.get("arm") or "").strip().lower()
|
||||
grip = robot.get("gripper") if isinstance(robot.get("gripper"), dict) else {}
|
||||
model = str(grip.get("model") or "")
|
||||
shared = (data or {}).get("gripper") if isinstance((data or {}).get("gripper"), dict) else {}
|
||||
stroke = grip.get("stroke_mm")
|
||||
if stroke is None:
|
||||
stroke = shared.get("stroke_mm") or 20.0
|
||||
out.append({
|
||||
"id": arm_no,
|
||||
"label": str(robot.get("label") or "").strip() or f"臂{arm_no}",
|
||||
"sn": sn,
|
||||
"side": side if side in ("left", "right") else "",
|
||||
"tactile": bool(robot.get("tactile")),
|
||||
"camera": str((robot.get("camera") or {}).get("name") or "") if isinstance(robot.get("camera"), dict) else "",
|
||||
"gripper": model,
|
||||
"rotary": "RGI" in model.upper(),
|
||||
"iface": str(robot.get("iface") or ""),
|
||||
"pc_ip": str(robot.get("pc_ip") or ""),
|
||||
"port": str(grip.get("port") or ""),
|
||||
"stroke_mm": float(stroke),
|
||||
})
|
||||
return out
|
||||
|
||||
|
||||
def usb_expectations(data: dict[str, Any] | None) -> dict[str, int]:
|
||||
"""设备页上「应有几台」跟 ``site.yaml`` 走，不写死本机的 2+1+2。"""
|
||||
cameras = (data or {}).get("cameras") or {}
|
||||
d405 = 0
|
||||
d435 = 0
|
||||
if isinstance(cameras, dict):
|
||||
for key, val in cameras.items():
|
||||
model = str((val or {}).get("model") or "") if isinstance(val, dict) else ""
|
||||
token = f"{key} {model}".lower()
|
||||
if "d405" in token:
|
||||
d405 += 1
|
||||
elif "d435" in token:
|
||||
d435 += 1
|
||||
tactile = (data or {}).get("tactile") or {}
|
||||
orisys = 0
|
||||
if isinstance(tactile, dict):
|
||||
orisys = sum(1 for key in ("left_video", "right_video") if tactile.get(key) is not None)
|
||||
return {"d405": d405, "d435": d435, "orisys": orisys}
|
||||
|
||||
|
||||
def load_site_dict(path: str | Path | None = None) -> dict[str, Any]:
|
||||
p = Path(path).expanduser() if path else site_path()
|
||||
if not p.is_file():
|
||||
return {}
|
||||
data = yaml.safe_load(p.read_text(encoding="utf-8")) or {}
|
||||
if not isinstance(data, dict):
|
||||
return {}
|
||||
resolved = p.resolve()
|
||||
root = resolved.parent.parent if resolved.parent.name == "config" else ros2_root()
|
||||
return absolutize_site_paths(data, root)
|
||||
|
||||
|
||||
def deep_merge(base: dict[str, Any], overlay: dict[str, Any]) -> dict[str, Any]:
|
||||
out = dict(base)
|
||||
for key, val in overlay.items():
|
||||
if isinstance(val, dict) and isinstance(out.get(key), dict):
|
||||
out[key] = deep_merge(out[key], val)
|
||||
else:
|
||||
out[key] = val
|
||||
return out
|
||||
|
||||
|
||||
def joint_prefix_default() -> str:
|
||||
"""当前臂的关节名前缀（``Rizon4-XXXXXX_``），用作 ROS 参数的默认值。
|
||||
|
||||
来源顺序：
|
||||
|
||||
1. ``SITE_JOINT_PREFIX`` —— ``scripts/env.sh`` → ``load_site_config.sh`` 从
|
||||
site.yaml 导出，这就是**权威值**；
|
||||
2. 由 ``SITE_ROBOT_SN`` 现算（``Rizon4-063989`` → ``Rizon4-063989_``）；
|
||||
3. 都没有 → 空字符串。
|
||||
|
||||
**绝不写死某个具体 SN。** 以前这里的默认值是 ``"Rizon4-063989_"``，
|
||||
于是换台机器人后 ``ros2 run`` 直接起的节点会去订**上一台臂**的关节话题，
|
||||
表现为"节点起来了、什么都不动、也不报错"（R1.1）。
|
||||
"""
|
||||
prefix = os.environ.get("SITE_JOINT_PREFIX", "").strip()
|
||||
if prefix:
|
||||
return prefix
|
||||
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
|
||||
return f"{sn}_" if sn else ""
|
||||
|
||||
|
||||
def base_frame_default() -> str:
|
||||
"""当前臂的基座坐标系（``<SN>_base_link``），用作 ROS 参数默认值。"""
|
||||
frame = os.environ.get("SITE_BASE_FRAME", "").strip()
|
||||
if frame:
|
||||
return frame
|
||||
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
|
||||
return f"{sn}_base_link" if sn else ""
|
||||
|
||||
|
||||
def ee_frame_default() -> str:
|
||||
"""当前臂的法兰坐标系（``<SN>_flange``），用作 ROS 参数默认值。"""
|
||||
frame = os.environ.get("SITE_EE_FRAME", "").strip()
|
||||
if frame:
|
||||
return frame
|
||||
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
|
||||
return f"{sn}_flange" if sn else ""
|
||||
|
||||
|
||||
def overhead_camera(
|
||||
site: dict[str, Any], robot: dict[str, Any] | None = None
|
||||
) -> tuple[dict[str, Any], list[str]]:
|
||||
"""手外（固定）相机 → ``(相机配置字典, 同样合格但未被选中的相机名)``。
|
||||
|
||||
手外相机 = ``cameras.*`` 里 ``role`` 以 ``overhead`` 开头的那台（眼在手外时
|
||||
相机固定在工位外，不随臂动）。
|
||||
|
||||
**为什么必须有唯一实现**：这份"按 role 找，再按 arm 收敛"的规则有两个调用方——
|
||||
S03 粗定位取命名空间（本模块给节点用）、手眼标定脚本取相机名。两边各写一份的
|
||||
后果不是报错，而是"标定用的相机"和"推理用的相机"悄悄不是同一台，且没人发现。
|
||||
所以规则只写在这里，两边都调它（`export_site_env.overhead_camera` 也走这里）。
|
||||
|
||||
收敛规则（确定性的三级）：
|
||||
1. ``arm`` 与当前臂相同的优先（仿真 ``sim_d435`` 靠这级选中）；
|
||||
2. 都没有 ``arm``（= 双臂共用的那台）时回落到它；
|
||||
3. 还是多个 → 取第一个，并把其余名字放进第二个返回值，让调用方**出声告警**。
|
||||
"""
|
||||
cams = [
|
||||
cfg
|
||||
for cfg in (site.get("cameras") or {}).values()
|
||||
if isinstance(cfg, dict)
|
||||
and str(cfg.get("role") or "").strip().lower().startswith("overhead")
|
||||
]
|
||||
if not cams:
|
||||
return {}, []
|
||||
arm = str((robot or {}).get("arm") or "").strip()
|
||||
mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else []
|
||||
hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams
|
||||
chosen = dict(hits[0])
|
||||
others = [str(c.get("name") or "?") for c in hits[1:]]
|
||||
return chosen, others
|
||||
|
||||
|
||||
def eye_to_hand_result_name(short: str, camera_name: str) -> str:
|
||||
"""眼在手外结果文件的**文件名**（不含目录）。
|
||||
|
||||
外参绑定「这条臂的基座 + 这台相机」：换条臂或换台相机都不通用，
|
||||
所以名字必须同时带上臂号与相机。调用方只许调这里，不许自己拼
|
||||
（launch / export_site_env / S03 找文件都走这一处）。
|
||||
"""
|
||||
short = str(short or "").strip()
|
||||
cam = str(camera_name or "").strip().strip("/")
|
||||
if short and cam:
|
||||
return f"{short}_{cam}_eye_to_hand.yaml"
|
||||
if cam:
|
||||
return f"{cam}_eye_to_hand.yaml"
|
||||
if short:
|
||||
return f"{short}_eye_to_hand.yaml"
|
||||
return "eye_to_hand.yaml"
|
||||
|
||||
|
||||
def resolve_eye_to_hand_file(
|
||||
root: Path,
|
||||
*,
|
||||
explicit: str = "",
|
||||
camera_name: str = "",
|
||||
) -> Path | None:
|
||||
"""找手外相机的手眼文件。没有就返回 None。
|
||||
|
||||
路径一律按 ``root`` 解析，不跟当前工作目录走——相对 CWD 的路径会在
|
||||
"换个目录启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。
|
||||
|
||||
候选顺序：
|
||||
1. ``explicit`` —— launch 按 site.yaml 派生的确切路径
|
||||
（``<臂号>_<相机>_eye_to_hand.yaml``）；
|
||||
2. ``<相机名>_eye_to_hand.yaml`` —— 旧命名 / 仿真（``sim_d435_...``）；
|
||||
3. ``eye_to_hand.yaml`` —— 再兜底。
|
||||
"""
|
||||
candidates: list[Path] = []
|
||||
if explicit:
|
||||
p = Path(explicit).expanduser()
|
||||
candidates.append(p if p.is_absolute() else root / p)
|
||||
cam = str(camera_name or "").strip().strip("/")
|
||||
if cam:
|
||||
candidates.append(root / "handeye" / "results" / f"{cam}_eye_to_hand.yaml")
|
||||
candidates.append(root / "handeye" / "results" / "eye_to_hand.yaml")
|
||||
for cand in candidates:
|
||||
if cand.is_file():
|
||||
return cand
|
||||
return None
|
||||
|
||||
|
||||
TRACK_CAMERA_ROLES = ("wrist", "overhead")
|
||||
|
||||
|
||||
def track_camera_role(site: dict[str, Any] | None) -> str:
|
||||
"""一阶段跟踪用哪台相机：``wrist``（腕部 D405）或 ``overhead``（顶置 D435）。
|
||||
|
||||
写错**当场报错**，不会退回腕部 —— 退回会让人以为在验 D435，其实还在用 D405。
|
||||
缺省 ``wrist``：没写这个键时行为与改之前一致（腕部追踪）。
|
||||
"""
|
||||
perc = (site or {}).get("perception")
|
||||
perc = perc if isinstance(perc, dict) else {}
|
||||
raw = perc.get("track_camera")
|
||||
if raw is None or str(raw).strip() == "":
|
||||
return "wrist"
|
||||
role = str(raw).strip().lower()
|
||||
if role not in TRACK_CAMERA_ROLES:
|
||||
raise ValueError(
|
||||
f"perception.track_camera 只能是 wrist 或 overhead，收到 {raw!r}。"
|
||||
"写错不会退回腕部 —— 退回会让人以为在用 D435 其实还在用 D405。"
|
||||
)
|
||||
return role
|
||||
|
||||
|
||||
def track_handeye_mode(role: str) -> str:
|
||||
"""跟踪相机角色 → 手眼装法。overhead 必须用 cam→基座，不能用 cam→法兰。"""
|
||||
if str(role or "").strip().lower() == "overhead":
|
||||
return "eye_to_hand"
|
||||
return "eye_in_hand"
|
||||
|
||||
|
||||
def handeye_static_tf_frames(
|
||||
*,
|
||||
mode: str,
|
||||
ee_frame: str,
|
||||
base_frame: str,
|
||||
cam_frame: str,
|
||||
) -> tuple[str, str]:
|
||||
"""手眼静态 TF 的 ``(父系, 子系)``。
|
||||
|
||||
两种装法的父子关系**不同**，不能混：
|
||||
* ``eye_in_hand``：法兰 ← 相机（相机跟着臂动）
|
||||
* ``eye_to_hand``：基座 ← 相机（相机固定，不随臂动）
|
||||
|
||||
把眼在手外的文件按"法兰 ← 相机"发出去，会得到一条**看着合理**的错 TF ——
|
||||
定位总能到附近、永远差一截，而人只会去怀疑标定精度。
|
||||
"""
|
||||
if str(mode or "").strip().lower() == "eye_to_hand":
|
||||
return str(base_frame), str(cam_frame)
|
||||
return str(ee_frame), str(cam_frame)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,84 @@
|
||||
"""触觉采样 / 指令类型。后端（Orisys / mock）可替换。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from abc import ABC, abstractmethod
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class TactileSample:
|
||||
timestamp: float
|
||||
force_xyz: np.ndarray
|
||||
torque_xyz: np.ndarray
|
||||
contact: bool
|
||||
embedding: np.ndarray | None = None
|
||||
raw: dict[str, Any] | None = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class TactileCommand:
|
||||
delta_xyz: np.ndarray
|
||||
delta_rpy: np.ndarray
|
||||
gripper_pos: float | None = None
|
||||
done: bool = False
|
||||
ok: bool = True
|
||||
message: str = ""
|
||||
|
||||
|
||||
class TactileInterface(ABC):
|
||||
name: str = "tactile_base"
|
||||
|
||||
@abstractmethod
|
||||
def connect(self) -> None: ...
|
||||
|
||||
@abstractmethod
|
||||
def disconnect(self) -> None: ...
|
||||
|
||||
@abstractmethod
|
||||
def reset(self, hole_id: int) -> None: ...
|
||||
|
||||
@abstractmethod
|
||||
def step(self, sample: TactileSample) -> TactileCommand: ...
|
||||
|
||||
|
||||
class MockTactile(TactileInterface):
|
||||
name = "mock_tactile"
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._i = 0
|
||||
self._hole = 0
|
||||
|
||||
def connect(self) -> None:
|
||||
self._i = 0
|
||||
|
||||
def disconnect(self) -> None:
|
||||
pass
|
||||
|
||||
def reset(self, hole_id: int) -> None:
|
||||
self._hole = hole_id
|
||||
self._i = 0
|
||||
|
||||
def read_sample(self) -> TactileSample:
|
||||
self._i += 1
|
||||
return TactileSample(
|
||||
timestamp=time.time(),
|
||||
force_xyz=np.zeros(3),
|
||||
torque_xyz=np.zeros(3),
|
||||
contact=self._i >= 8,
|
||||
)
|
||||
|
||||
def step(self, sample: TactileSample) -> TactileCommand:
|
||||
self._i += 1
|
||||
done = self._i >= 15
|
||||
return TactileCommand(
|
||||
delta_xyz=np.zeros(3),
|
||||
delta_rpy=np.zeros(3),
|
||||
done=done,
|
||||
ok=True,
|
||||
message=f"mock hole={self._hole} step={self._i}",
|
||||
)
|
||||
@@ -0,0 +1,305 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Publish Orisys dual-finger tactile status. Config from site.yaml tactile:.
|
||||
|
||||
架构（厂家示例 two_sensor_pipeline 的推荐）：每个传感器一个独立
|
||||
multiprocessing 进程（独立 CUDA 上下文），ROS 主进程只做队列→话题转发。
|
||||
原因：实测单进程内双开两路传感器会互相拖垮（30→0.8fps）；独立进程稳定 30fps。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import multiprocessing as mp
|
||||
import signal
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_application.image_utils import UnsupportedImageError, ndarray_to_image
|
||||
from arm_application.orisys_tactile import load_orisys_from_site, tactile_worker
|
||||
from arm_application.site_io import load_site_dict, site_path
|
||||
from arm_msgs.msg import TactileStatus
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
class TactileNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("tactile")
|
||||
self.declare_parameter("publish_hz", 20.0)
|
||||
# 下面两个可以用 ROS 参数覆盖 site.yaml，便于不改站点文件就做验证
|
||||
# （例如 mock 后端 + 打开图像话题来确认链路通）。
|
||||
self.declare_parameter("backend", "")
|
||||
self.declare_parameter("image_hz", -1.0)
|
||||
data = load_site_dict()
|
||||
t = data.get("tactile") or {}
|
||||
hz = float(t.get("publish_hz") or self.get_parameter("publish_hz").value or 20.0)
|
||||
backend_override = str(self.get_parameter("backend").value).strip()
|
||||
self._backend = (
|
||||
backend_override or str(t.get("backend") or "orisys")
|
||||
).strip().lower()
|
||||
self._mock = self._backend in ("mock", "mock_orisys")
|
||||
|
||||
self._pub = {
|
||||
side: self.create_publisher(TactileStatus, f"/tactile/{side}/status", 10)
|
||||
for side in ("left", "right")
|
||||
}
|
||||
self._latest: dict[str, tuple[float, dict]] = {}
|
||||
self._connected = {"left": False, "right": False}
|
||||
self._mock_n = 0
|
||||
self._workers: list = []
|
||||
self._queues: dict[str, object] = {}
|
||||
self._stop = None
|
||||
|
||||
# --- 视触觉图像 / 力矢量场 ---
|
||||
# 默认关闭（image_hz=0）：图像每帧几百 KB，不该拖累三力的 30fps 时序。
|
||||
# 打开后走一条 maxsize=1 的独立队列，满则丢旧留新 —— 主进程来不及消费
|
||||
# 只会丢图像，三力一条不受影响。
|
||||
self._image_hz = (
|
||||
float(self.get_parameter("image_hz").value)
|
||||
if float(self.get_parameter("image_hz").value) >= 0
|
||||
else float(t.get("image_hz") or 0.0)
|
||||
)
|
||||
self._image_q = None
|
||||
self._image_warned: set[str] = set()
|
||||
self._image_pub: dict[str, object] = {}
|
||||
self._vector_pub: dict[str, object] = {}
|
||||
self._image_count: dict[str, int] = {}
|
||||
self._last_mock_img = 0.0
|
||||
|
||||
# 图像 publisher 与后端无关 —— mock 后端也要能发。
|
||||
# 否则"图像话题到底通不通"只能等到有相机、上真机那天才发现。
|
||||
if self._image_hz > 0:
|
||||
for side in ("left", "right"):
|
||||
self._image_pub[side] = self.create_publisher(
|
||||
Image, f"/tactile/{side}/image", 5
|
||||
)
|
||||
self._vector_pub[side] = self.create_publisher(
|
||||
Image, f"/tactile/{side}/vector", 5
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"视触觉图像已开启: {self._image_hz:.1f}Hz "
|
||||
f"→ /tactile/{{left,right}}/image 与 /vector"
|
||||
)
|
||||
|
||||
if self._mock:
|
||||
self._connected = {"left": True, "right": True}
|
||||
self.get_logger().info("触觉使用 mock 后端（假数据，不接真实传感器）")
|
||||
else:
|
||||
cfg = load_orisys_from_site(data)
|
||||
ctx = mp.get_context("spawn") # spawn：子进程不带 DDS 线程，CUDA 上下文干净
|
||||
self._stop = ctx.Event()
|
||||
if self._image_hz > 0:
|
||||
self._image_q = ctx.Queue(maxsize=1)
|
||||
for side in ("left", "right"):
|
||||
q = ctx.Queue(maxsize=2)
|
||||
self._queues[side] = q
|
||||
p = ctx.Process(
|
||||
target=tactile_worker,
|
||||
args=(side, cfg, q, self._stop, self._image_q),
|
||||
name=f"tactile_{side}", daemon=True,
|
||||
)
|
||||
p.start()
|
||||
self._workers.append(p)
|
||||
self.get_logger().info(
|
||||
f"tactile workers spawned (left pid={self._workers[0].pid} right pid={self._workers[1].pid}) "
|
||||
f"site={site_path()}"
|
||||
)
|
||||
self.create_timer(1.0 / max(1e-3, hz), self._tick)
|
||||
|
||||
def _drain(self, side: str) -> None:
|
||||
q = self._queues.get(side)
|
||||
if q is None:
|
||||
return
|
||||
latest = None
|
||||
while True:
|
||||
try:
|
||||
latest = q.get_nowait()
|
||||
except Exception:
|
||||
break
|
||||
if latest is None:
|
||||
return
|
||||
if latest.get("error"):
|
||||
self.get_logger().error(f"[{side}] {latest['error']}", throttle_duration_sec=3.0)
|
||||
return
|
||||
self._connected[side] = True
|
||||
self._latest[side] = (time.monotonic(), latest)
|
||||
|
||||
def _drain_images(self) -> None:
|
||||
"""把 worker 推来的最新图像 / 力矢量场发布出去。
|
||||
|
||||
只取最新一帧：worker 已按 ``image_hz`` 节流、队列 ``maxsize=1``，
|
||||
所以这里读到的永远是最新画面；主进程偶尔卡一下，丢掉的是中间帧
|
||||
而不是最新帧。
|
||||
"""
|
||||
if self._image_q is None:
|
||||
return
|
||||
latest = None
|
||||
while True:
|
||||
try:
|
||||
latest = self._image_q.get_nowait()
|
||||
except Exception:
|
||||
break
|
||||
if latest is None:
|
||||
return
|
||||
|
||||
side = str(latest.get("side") or "")
|
||||
if side not in self._image_pub:
|
||||
return
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
frame_id = f"{side}_tactile"
|
||||
|
||||
self._publish_array(
|
||||
self._image_pub[side], latest.get("img"), frame_id, stamp, side, "image"
|
||||
)
|
||||
self._publish_array(
|
||||
self._vector_pub[side], latest.get("vector"), frame_id, stamp, side, "vector"
|
||||
)
|
||||
|
||||
def _publish_array(self, pub, array, frame_id: str, stamp, side: str, tag: str) -> None:
|
||||
"""打包并发布一个数组；形状/类型不支持时跳过该话题而不是崩溃。
|
||||
|
||||
``array is None`` 是正常情况（该侧没有矢量场输出、或本帧没算），
|
||||
不报错也不发空图 —— 发一张全零的假图会让下游以为接触到了东西。
|
||||
"""
|
||||
if array is None:
|
||||
return
|
||||
key = f"{side}_{tag}"
|
||||
try:
|
||||
msg = ndarray_to_image(array, frame_id=frame_id, stamp=stamp)
|
||||
except UnsupportedImageError as exc:
|
||||
# 编码不支持是固定的（SDK 给的 dtype 不会变），每侧每种图只提醒一次，
|
||||
# 否则 5Hz 的警告会把现场日志冲掉。
|
||||
if key not in self._image_warned:
|
||||
self._image_warned.add(key)
|
||||
self.get_logger().warn(
|
||||
f"[{key}] 无法打包成 Image，已跳过该话题: {exc}"
|
||||
)
|
||||
return
|
||||
|
||||
try:
|
||||
pub.publish(msg)
|
||||
except Exception as exc:
|
||||
if f"{key}_pub" not in self._image_warned:
|
||||
self._image_warned.add(f"{key}_pub")
|
||||
self.get_logger().warn(f"[{key}] 发布失败: {exc}")
|
||||
return
|
||||
|
||||
if self._image_count.get(key, 0) == 0:
|
||||
self.get_logger().info(
|
||||
f"[{key}] 首帧已发布 ({msg.width}x{msg.height} {msg.encoding})"
|
||||
)
|
||||
self._image_count[key] = self._image_count.get(key, 0) + 1
|
||||
|
||||
def _publish_mock_images(self, force: float) -> None:
|
||||
"""mock 后端下造一张形变图 + 一张力矢量场，用来打通图像发布链路。
|
||||
|
||||
没有传感器时也要能把 ``/tactile/<side>/image`` 与 ``/vector`` 跑起来 ——
|
||||
否则这两个话题是否真的能出图，只能等到上真机那天才知道。
|
||||
|
||||
图像内容模仿视触觉的形变：中间一个高斯隆起（法向深度），
|
||||
连同指向中心的切向流（矢量场），接触越深幅度越大。
|
||||
"""
|
||||
if not self._image_pub:
|
||||
return
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
h, w = 48, 64
|
||||
yy, xx = np.mgrid[0:h, 0:w]
|
||||
cx, cy = w / 2.0, h / 2.0
|
||||
bump = np.exp(-(((xx - cx) ** 2 + (yy - cy) ** 2) / (2.0 * 9.0 ** 2)))
|
||||
|
||||
depth = (bump * (0.05 + 0.5 * float(force))).astype(np.float32)
|
||||
vx = ((cx - xx) / w * bump * float(force)).astype(np.float32)
|
||||
vy = ((cy - yy) / h * bump * float(force)).astype(np.float32)
|
||||
vector = np.dstack([vx, vy])
|
||||
|
||||
for side in ("left", "right"):
|
||||
frame = f"{side}_tactile"
|
||||
self._publish_array(self._image_pub[side], depth, frame, stamp, side, "image")
|
||||
self._publish_array(self._vector_pub[side], vector, frame, stamp, side, "vector")
|
||||
|
||||
def _tick(self) -> None:
|
||||
self._drain_images()
|
||||
if self._mock:
|
||||
self._mock_n += 1
|
||||
force = 0.2 if self._mock_n >= 8 else 0.0
|
||||
for side in ("left", "right"):
|
||||
self._publish(side, {
|
||||
"fnormal": force, "fshearx": 0.0, "fsheary": 0.0,
|
||||
"fps": 30.0, "is_contact": self._mock_n >= 8,
|
||||
"centroid": (0.0, 0.0),
|
||||
})
|
||||
# mock 的图像也按 image_hz 节流，避免比真机行为更"勤快"。
|
||||
if self._image_hz > 0:
|
||||
now = time.monotonic()
|
||||
if now - self._last_mock_img >= 1.0 / self._image_hz:
|
||||
self._last_mock_img = now
|
||||
self._publish_mock_images(force)
|
||||
return
|
||||
for side in ("left", "right"):
|
||||
self._drain(side)
|
||||
item = self._latest.get(side)
|
||||
if item is None:
|
||||
continue
|
||||
ts, data = item
|
||||
if time.monotonic() - ts > 0.6:
|
||||
continue # 数据太旧（worker 卡住）不发布
|
||||
self._publish(side, data)
|
||||
|
||||
def _publish(self, side: str, data: dict) -> None:
|
||||
msg = TactileStatus()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = f"{side}_tactile"
|
||||
msg.side = side
|
||||
msg.connected = bool(self._connected.get(side, False))
|
||||
msg.contact = bool(data.get("is_contact", False))
|
||||
msg.fnormal = float(data.get("fnormal") or 0.0)
|
||||
msg.fshearx = float(data.get("fshearx") or 0.0)
|
||||
msg.fsheary = float(data.get("fsheary") or 0.0)
|
||||
msg.fps = float(data.get("fps") or 0.0)
|
||||
c = data.get("centroid") or (0.0, 0.0)
|
||||
msg.centroid_u = float(c[0])
|
||||
msg.centroid_v = float(c[1])
|
||||
msg.error_msg = ""
|
||||
self._pub[side].publish(msg)
|
||||
|
||||
def destroy_node(self) -> bool:
|
||||
try:
|
||||
if self._stop is not None:
|
||||
self._stop.set()
|
||||
for p in self._workers:
|
||||
p.join(timeout=2.0)
|
||||
if p.is_alive():
|
||||
p.terminate()
|
||||
for p in self._workers:
|
||||
p.join(timeout=1.5)
|
||||
if p.is_alive():
|
||||
try:
|
||||
p.kill() # 还赖着就 SIGKILL，绝不留孤儿占摄像头
|
||||
except Exception:
|
||||
pass
|
||||
finally:
|
||||
return super().destroy_node()
|
||||
|
||||
|
||||
def main() -> None:
|
||||
rclpy.init()
|
||||
node = TactileNode()
|
||||
|
||||
def _sigterm(_sig, _frm) -> None:
|
||||
raise KeyboardInterrupt
|
||||
|
||||
signal.signal(signal.SIGTERM, _sigterm) # SIGTERM 也走干净退出，释放相机
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,618 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Ultralytics YOLO / TensorRT 封装。
|
||||
|
||||
对外接口：``resolve_weights``、``YoloDetector``、``Detection``、``pick_best``、
|
||||
``MpYoloDetector``。当 ``backend='tensorrt'`` 时，TensorRT 会加载与
|
||||
``.onnx`` / ``.pt`` 路径同级的 ``.engine`` 文件。
|
||||
|
||||
类别名（标签）由 ``model_labels`` 统一提供：优先读权重旁边的
|
||||
``<模型名>.labels.yaml``（用户可随时手改），其次读模型内嵌 metadata。
|
||||
这样做是因为 TensorRT engine 在当前的 TensorRT Python API 下**无法携带标签**，
|
||||
只能靠旁挂文件；详见 ``model_labels`` 模块头。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import multiprocessing as mp
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Sequence, Union
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .model_labels import (
|
||||
SOURCE_PLACEHOLDER,
|
||||
LabelSet,
|
||||
describe,
|
||||
parse_names_field,
|
||||
placeholder_labels,
|
||||
read_labels,
|
||||
)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# 哨兵值：worker 还停在上一次的那帧 —— 调用方不能把它算作漏检。
|
||||
INFER_BUSY = object()
|
||||
|
||||
|
||||
@dataclass
|
||||
class Detection:
|
||||
xyxy: np.ndarray # (4,) float：x1,y1,x2,y2
|
||||
conf: float
|
||||
cls_id: int
|
||||
cls_name: str
|
||||
# pose 模型才有：(K, 3) = 每个关键点的 x, y, conf（**原图像素坐标**）。
|
||||
# 检测模型恒为 None —— 下游必须先判 None，别假设它有。
|
||||
kpts: Optional[np.ndarray] = None
|
||||
|
||||
@property
|
||||
def cx(self) -> float:
|
||||
return float(0.5 * (self.xyxy[0] + self.xyxy[2]))
|
||||
|
||||
@property
|
||||
def cy(self) -> float:
|
||||
return float(0.5 * (self.xyxy[1] + self.xyxy[3]))
|
||||
|
||||
@property
|
||||
def area(self) -> float:
|
||||
return float(max(0.0, self.xyxy[2] - self.xyxy[0]) * max(0.0, self.xyxy[3] - self.xyxy[1]))
|
||||
|
||||
|
||||
def _resolve_device(device: str) -> str:
|
||||
try:
|
||||
import torch
|
||||
except Exception: # noqa: BLE001
|
||||
return "cpu"
|
||||
d = (device or "").strip().lower()
|
||||
if d in ("", "auto", "cuda", "cuda:0", "0", "gpu"):
|
||||
return "0" if torch.cuda.is_available() else "cpu"
|
||||
return device.strip() or "cpu"
|
||||
|
||||
|
||||
def resolve_weights(weights: Union[str, Path], backend: str = "auto") -> Path:
|
||||
"""当 backend=auto/tensorrt 时，优先使用与 .pt 同级的 TensorRT .engine。"""
|
||||
p = Path(weights).expanduser()
|
||||
b = (backend or "auto").strip().lower()
|
||||
engine = p.with_suffix(".engine")
|
||||
onnx = p.with_suffix(".onnx")
|
||||
if b in ("tensorrt", "engine"):
|
||||
if engine.is_file():
|
||||
return engine
|
||||
raise FileNotFoundError(
|
||||
f"缺少 TensorRT engine：{engine}。"
|
||||
f"导出命令：bash ~/ros2/scripts/weights.sh convert {p} --name {p.stem}"
|
||||
)
|
||||
if b == "onnx":
|
||||
if onnx.is_file():
|
||||
return onnx
|
||||
raise FileNotFoundError(f"缺少 ONNX 文件：{onnx}")
|
||||
if b in ("", "auto"):
|
||||
if engine.is_file():
|
||||
return engine
|
||||
if p.is_file():
|
||||
return p
|
||||
if onnx.is_file():
|
||||
return onnx
|
||||
return p
|
||||
|
||||
|
||||
def resolve_class_labels(path: Union[str, Path], model=None) -> LabelSet:
|
||||
"""给出一个权重该用的类别标签（旁挂文件优先）。
|
||||
|
||||
``model`` 是已经加载好的 Ultralytics 模型对象（可为 None）。顺序：
|
||||
|
||||
1. ``<模型名>.labels.yaml`` —— 用户可编辑的权威来源
|
||||
2. 传入的 ``model.names`` —— 模型内嵌 metadata
|
||||
3. 占位符 ``class_0 / class_1 …``
|
||||
|
||||
第 3 步刻意不用 ``str(cid)``。原先读不到名字时会退化成类名 `"0"`，
|
||||
和"真的有个叫 0 的类"无法区分，标签丢了都看不出来。
|
||||
占位符 + 一条 warning 让这件事在日志里可见。
|
||||
"""
|
||||
p = Path(path).expanduser()
|
||||
|
||||
# 先只看旁挂文件（快，不加载模型）：它必须能覆盖模型内嵌的旧值，
|
||||
# 否则"我改了标签为什么不生效"会变成很难查的问题。
|
||||
labels = read_labels(p, allow_model_metadata=False)
|
||||
if not labels.is_placeholder:
|
||||
return labels
|
||||
|
||||
raw = getattr(model, "names", None) if model is not None else None
|
||||
names = parse_names_field(raw or {})
|
||||
if names:
|
||||
from .model_labels import SOURCE_MODEL
|
||||
|
||||
return LabelSet(names=names, source=SOURCE_MODEL, path=p)
|
||||
|
||||
nc = 0
|
||||
try:
|
||||
nc = int(getattr(model, "nc", 0) or 0)
|
||||
except (TypeError, ValueError):
|
||||
nc = 0
|
||||
labels = LabelSet(names=placeholder_labels(nc), source=SOURCE_PLACEHOLDER, path=None)
|
||||
logger.warning(
|
||||
"模型 %s 没有类别标签，暂时用占位名显示。补一个 %s 即可（改完重跑节点生效）。"
|
||||
"命令行: bash ~/ros2/scripts/weights.sh labels %s --from-model",
|
||||
p.name,
|
||||
p.stem + ".labels.yaml",
|
||||
p.stem,
|
||||
)
|
||||
return labels
|
||||
|
||||
|
||||
class YoloDetector:
|
||||
def __init__(
|
||||
self,
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
conf: float = 0.5,
|
||||
iou: float = 0.45,
|
||||
device: str = "",
|
||||
imgsz: int = 640,
|
||||
class_ids: Optional[Sequence[int]] = None,
|
||||
class_names: Optional[Sequence[str]] = None,
|
||||
) -> None:
|
||||
try:
|
||||
from ultralytics import YOLO
|
||||
except Exception as exc: # noqa: BLE001
|
||||
raise RuntimeError(
|
||||
"未安装 ultralytics。在 ros_flexiv 环境里执行：pip install ultralytics"
|
||||
) from exc
|
||||
|
||||
path = Path(weights).expanduser()
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(
|
||||
f"找不到 YOLO 权重：{path}。"
|
||||
f"请把你的 .pt 放到 ~/ros2/models/yolo/workpiece.pt"
|
||||
)
|
||||
try:
|
||||
import torch
|
||||
|
||||
# 别让 PyTorch 占满所有 CPU 核心（相机 + ROS 都要用）。
|
||||
torch.set_num_threads(1)
|
||||
torch.set_num_interop_threads(1)
|
||||
if torch.cuda.is_available():
|
||||
torch.backends.cudnn.benchmark = True
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
self.device = _resolve_device(device)
|
||||
self.conf = float(conf)
|
||||
self.iou = float(iou)
|
||||
self.imgsz = int(imgsz)
|
||||
self._half_ok = False
|
||||
self._is_engine = path.suffix.lower() in {".engine", ".trt"}
|
||||
self._model = YOLO(str(path), task="detect")
|
||||
if not self._is_engine:
|
||||
try:
|
||||
self._model.fuse()
|
||||
except Exception:
|
||||
pass
|
||||
self.class_ids = set(int(v) for v in class_ids) if class_ids else None
|
||||
self.class_names = (
|
||||
{str(n).strip().lower() for n in class_names if str(n).strip()}
|
||||
if class_names
|
||||
else None
|
||||
)
|
||||
self._labels = resolve_class_labels(path, self._model)
|
||||
self._id_to_name = dict(self._labels.names)
|
||||
self._warmed = False
|
||||
self._pending = None
|
||||
|
||||
def warmup(self, *, half: bool = False) -> None:
|
||||
if self._warmed:
|
||||
return
|
||||
if half and (not self._is_engine) and self.device != "cpu":
|
||||
try:
|
||||
self._model.to("cuda")
|
||||
self._model.model.half()
|
||||
self._half_ok = True
|
||||
except Exception:
|
||||
self._half_ok = False
|
||||
dummy = np.zeros((self.imgsz, self.imgsz, 3), dtype=np.uint8)
|
||||
self._predict(dummy)
|
||||
self._warmed = True
|
||||
|
||||
def _predict(self, image_bgr: np.ndarray):
|
||||
kwargs = dict(
|
||||
source=image_bgr,
|
||||
conf=self.conf,
|
||||
iou=self.iou,
|
||||
imgsz=self.imgsz,
|
||||
device=self.device,
|
||||
verbose=False,
|
||||
max_det=20,
|
||||
)
|
||||
if self._half_ok and not self._is_engine:
|
||||
kwargs["quantize"] = 16
|
||||
try:
|
||||
import torch
|
||||
|
||||
ctx = torch.inference_mode()
|
||||
except Exception:
|
||||
from contextlib import nullcontext
|
||||
|
||||
ctx = nullcontext()
|
||||
with ctx:
|
||||
return self._model.predict(**kwargs)
|
||||
|
||||
def detect(
|
||||
self,
|
||||
image_bgr: np.ndarray,
|
||||
conf: float | None = None,
|
||||
iou: float | None = None,
|
||||
imgsz: int | None = None,
|
||||
**_kwargs,
|
||||
) -> List[Detection]:
|
||||
"""给孔检测器用的别名接口（xyxy + conf）。"""
|
||||
if conf is not None:
|
||||
self.conf = float(conf)
|
||||
if iou is not None:
|
||||
self.iou = float(iou)
|
||||
if imgsz is not None:
|
||||
self.imgsz = int(imgsz)
|
||||
return self.infer(image_bgr)
|
||||
|
||||
def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0) -> List[Detection]:
|
||||
del seq
|
||||
if not self._warmed:
|
||||
self.warmup(half=half)
|
||||
results = self._predict(image_bgr)
|
||||
if not results:
|
||||
return []
|
||||
r0 = results[0]
|
||||
boxes = getattr(r0, "boxes", None)
|
||||
if boxes is None or len(boxes) == 0:
|
||||
return []
|
||||
|
||||
out: List[Detection] = []
|
||||
xyxy = boxes.xyxy.detach().cpu().numpy()
|
||||
confs = boxes.conf.detach().cpu().numpy()
|
||||
clss = boxes.cls.detach().cpu().numpy().astype(int)
|
||||
for i in range(len(xyxy)):
|
||||
cid = int(clss[i])
|
||||
# 用 LabelSet.get：读不到名字时给 class_N 占位，而不是退化成裸数字 "0"。
|
||||
name = self._labels.get(cid)
|
||||
if self.class_ids is not None and cid not in self.class_ids:
|
||||
continue
|
||||
if self.class_names is not None and name.lower() not in self.class_names:
|
||||
continue
|
||||
out.append(
|
||||
Detection(
|
||||
xyxy=xyxy[i].astype(float),
|
||||
conf=float(confs[i]),
|
||||
cls_id=cid,
|
||||
cls_name=name,
|
||||
)
|
||||
)
|
||||
out.sort(key=lambda d: d.conf, reverse=True)
|
||||
return out
|
||||
|
||||
def submit(self, image_bgr: np.ndarray, seq: int = 0) -> None:
|
||||
self._pending = (int(seq), image_bgr)
|
||||
|
||||
def wait_result(self, timeout: float = 0.045):
|
||||
del timeout
|
||||
if self._pending is None:
|
||||
return None
|
||||
seq, img = self._pending
|
||||
self._pending = None
|
||||
t0 = time.monotonic()
|
||||
dets = self.infer(img, seq=seq)
|
||||
return seq, dets, (time.monotonic() - t0) * 1000.0
|
||||
|
||||
|
||||
def build_detector(
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
backend: str = "auto",
|
||||
conf: float = 0.25,
|
||||
iou: float = 0.45,
|
||||
device: str = "",
|
||||
imgsz: int = 640,
|
||||
):
|
||||
"""按权重类型挑检测器：pose 模型走 ``PoseDetector``，否则走 ``YoloDetector``。
|
||||
|
||||
为什么要这个工厂：ultralytics 解不了 pose 的 TensorRT engine（见
|
||||
``pose_detector`` 模块头），所以 pose 必须换成自解码实现。判断依据是模型
|
||||
是不是 ``task=pose`` —— 直接读旁边 ``.onnx`` 的 metadata，不靠调用方声明。
|
||||
|
||||
返回对象统一有 ``detect()/infer()/warmup()/imgsz/class_names/class_ids``。
|
||||
"""
|
||||
path = Path(weights).expanduser()
|
||||
from .pose_detector import PoseDetector, is_pose_model
|
||||
|
||||
if is_pose_model(path):
|
||||
logger.info("检测到 pose 模型 %s → 用 PoseDetector（自解码关键点）", path.name)
|
||||
return PoseDetector(
|
||||
path,
|
||||
conf=conf,
|
||||
iou=iou,
|
||||
device=device or "0",
|
||||
imgsz=imgsz,
|
||||
)
|
||||
return YoloDetector(
|
||||
path,
|
||||
conf=conf,
|
||||
iou=iou,
|
||||
device=device,
|
||||
imgsz=imgsz,
|
||||
)
|
||||
|
||||
|
||||
def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
|
||||
"""spawn 出来的子进程：只跑 GPU 上的检测。这里绝不能 import rclpy。"""
|
||||
try:
|
||||
path = resolve_weights(cfg["weights"], cfg.get("backend") or "auto")
|
||||
det = build_detector(
|
||||
path,
|
||||
backend=cfg.get("backend") or "auto",
|
||||
conf=float(cfg.get("conf") or 0.25),
|
||||
iou=float(cfg.get("iou") or 0.45),
|
||||
device=str(cfg.get("device") or "0"),
|
||||
imgsz=int(cfg.get("imgsz") or 416),
|
||||
)
|
||||
det.warmup(half=bool(cfg.get("half", True)))
|
||||
q_out.put(("ready", str(path)))
|
||||
except Exception as exc: # noqa: BLE001
|
||||
q_out.put(("error", str(exc)))
|
||||
return
|
||||
while True:
|
||||
item = q_in.get()
|
||||
if item is None:
|
||||
return
|
||||
# 一直排空到队列里最新的帧 —— 绝不把 GPU 浪费在过期图像上。
|
||||
try:
|
||||
while True:
|
||||
newer = q_in.get_nowait()
|
||||
if newer is None:
|
||||
return
|
||||
item = newer
|
||||
except Exception:
|
||||
pass
|
||||
seq, image = item
|
||||
try:
|
||||
t0 = time.monotonic()
|
||||
dets = det.infer(image, half=bool(cfg.get("half", True)))
|
||||
gpu_ms = (time.monotonic() - t0) * 1000.0
|
||||
packed = [
|
||||
(
|
||||
d.xyxy.astype(np.float32),
|
||||
float(d.conf),
|
||||
int(d.cls_id),
|
||||
str(d.cls_name),
|
||||
None if d.kpts is None else np.asarray(d.kpts, dtype=np.float32),
|
||||
)
|
||||
for d in dets
|
||||
]
|
||||
_put_drop_old(q_out, ("ok", seq, packed, gpu_ms))
|
||||
except Exception as exc: # noqa: BLE001
|
||||
_put_drop_old(q_out, ("error", str(exc)))
|
||||
|
||||
|
||||
def _put_drop_old(q, item) -> None:
|
||||
"""绝不让 GPU 进程因为结果队列满了而阻塞。"""
|
||||
try:
|
||||
q.put_nowait(item)
|
||||
return
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
q.get_nowait()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
q.put_nowait(item)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
class YoloInferProcess:
|
||||
"""把 YOLO 放进 spawn() 子进程，让 ROS 回调永远不用和 PyTorch 抢 GIL。"""
|
||||
|
||||
def __init__(self, cfg: dict) -> None:
|
||||
self._cfg = dict(cfg)
|
||||
self._p = None
|
||||
self._start()
|
||||
|
||||
def _start(self) -> None:
|
||||
ctx = mp.get_context("spawn")
|
||||
# maxsize=2 + worker 排空到最新：父进程永远不需要主动丢帧
|
||||
# （之前 Queue put_nowait/get_nowait 的竞争，正是 worker 明明活着
|
||||
# 却出现数秒“静默”冻结的根源）。
|
||||
self._q_in = ctx.Queue(maxsize=2)
|
||||
self._q_out = ctx.Queue(maxsize=4)
|
||||
self._p = ctx.Process(
|
||||
target=_yolo_worker_main,
|
||||
args=(self._q_in, self._q_out, self._cfg),
|
||||
name="yolo_infer",
|
||||
daemon=True,
|
||||
)
|
||||
self._p.start()
|
||||
msg = self._q_out.get(timeout=90.0)
|
||||
if not msg or msg[0] != "ready":
|
||||
err = msg[1] if msg and len(msg) > 1 else "worker failed"
|
||||
raise RuntimeError(f"YOLO 工作进程：{err}")
|
||||
self.weights_path = str(msg[1])
|
||||
self.imgsz = int(self._cfg.get("imgsz") or 416)
|
||||
|
||||
def restart(self) -> None:
|
||||
"""杀掉卡死 / 挂掉的 worker，再起一个新的（重新加载 engine 约 2-5 s）。"""
|
||||
self.close()
|
||||
self._start()
|
||||
|
||||
@property
|
||||
def alive(self) -> bool:
|
||||
return bool(self._p is not None and self._p.is_alive())
|
||||
|
||||
def submit(self, image_bgr: np.ndarray, seq: int = 0) -> bool:
|
||||
"""把最新一帧排进队列。如果 worker 正忙就跳过 —— 它在每次推理后都会
|
||||
排空到队列里最新的帧，所以不会跑任何过期数据。
|
||||
|
||||
返回是否真的进队。进不去必须告诉调用方，否则一次性会以为「已经在跑」
|
||||
而空等，把 D405 也卡住。
|
||||
"""
|
||||
if not self.alive:
|
||||
raise RuntimeError("YOLO 工作进程已死亡")
|
||||
try:
|
||||
self._q_in.put((int(seq), image_bgr), timeout=0.010)
|
||||
return True
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
def wait_result(self, timeout: float = 0.045):
|
||||
"""返回 ``(seq, list[Detection], gpu_ms)`` 或 ``None``。"""
|
||||
if not self.alive:
|
||||
raise RuntimeError("YOLO 工作进程已死亡")
|
||||
deadline = time.monotonic() + max(0.004, float(timeout))
|
||||
latest = None
|
||||
while time.monotonic() < deadline:
|
||||
try:
|
||||
msg = self._q_out.get(
|
||||
timeout=max(0.001, deadline - time.monotonic())
|
||||
)
|
||||
except Exception:
|
||||
break
|
||||
latest = self._parse_out(msg)
|
||||
while True:
|
||||
try:
|
||||
extra = self._q_out.get_nowait()
|
||||
except Exception:
|
||||
break
|
||||
parsed = self._parse_out(extra)
|
||||
if parsed is not None:
|
||||
latest = parsed
|
||||
if latest is not None:
|
||||
break
|
||||
return latest
|
||||
|
||||
def _parse_out(self, msg):
|
||||
if not msg:
|
||||
return None
|
||||
if msg[0] == "error":
|
||||
raise RuntimeError(str(msg[1]))
|
||||
if msg[0] != "ok":
|
||||
return None
|
||||
seq, packed = int(msg[1]), msg[2]
|
||||
gpu_ms = float(msg[3]) if len(msg) > 3 else 0.0
|
||||
dets = []
|
||||
for row in packed:
|
||||
# 兼容旧格式（4 元组，无关键点），免得子进程和父进程版本不一致时炸掉。
|
||||
xyxy, conf, cid, name = row[0], row[1], row[2], row[3]
|
||||
kpts = row[4] if len(row) > 4 else None
|
||||
dets.append(
|
||||
Detection(
|
||||
xyxy=np.asarray(xyxy, dtype=float),
|
||||
conf=float(conf),
|
||||
cls_id=int(cid),
|
||||
cls_name=str(name),
|
||||
kpts=None if kpts is None else np.asarray(kpts, dtype=float),
|
||||
)
|
||||
)
|
||||
return seq, dets, gpu_ms
|
||||
|
||||
def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0):
|
||||
"""兼容接口：提交 + 等待。要做流水线的话优先用 submit/wait_result。"""
|
||||
del half
|
||||
self.submit(image_bgr, seq)
|
||||
got = self.wait_result(timeout=0.08)
|
||||
if got is None:
|
||||
return INFER_BUSY
|
||||
return got[1]
|
||||
|
||||
def close(self) -> None:
|
||||
if self._p is None:
|
||||
return
|
||||
try:
|
||||
self._q_in.put_nowait(None)
|
||||
except Exception:
|
||||
pass
|
||||
self._p.join(timeout=2.0)
|
||||
if self._p.is_alive():
|
||||
self._p.terminate()
|
||||
self._p.join(timeout=1.0)
|
||||
if self._p.is_alive():
|
||||
self._p.kill() # 卡在 CUDA 调用里出不来 —— 直接 SIGKILL
|
||||
self._p.join(timeout=1.0)
|
||||
self._p = None
|
||||
|
||||
|
||||
def _iou_xyxy(a: np.ndarray, b: np.ndarray) -> float:
|
||||
ax1, ay1, ax2, ay2 = [float(v) for v in a]
|
||||
bx1, by1, bx2, by2 = [float(v) for v in b]
|
||||
ix1, iy1 = max(ax1, bx1), max(ay1, by1)
|
||||
ix2, iy2 = min(ax2, bx2), min(ay2, by2)
|
||||
inter = max(0.0, ix2 - ix1) * max(0.0, iy2 - iy1)
|
||||
ua = (
|
||||
max(0.0, ax2 - ax1) * max(0.0, ay2 - ay1)
|
||||
+ max(0.0, bx2 - bx1) * max(0.0, by2 - by1)
|
||||
- inter
|
||||
)
|
||||
return float(inter / ua) if ua > 1e-6 else 0.0
|
||||
|
||||
|
||||
def pick_best(
|
||||
dets: Sequence[Detection],
|
||||
*,
|
||||
prefer_center: Optional[tuple] = None,
|
||||
prev_uv: Optional[np.ndarray] = None,
|
||||
prev_xyxy: Optional[np.ndarray] = None,
|
||||
max_track_px: float = 320.0,
|
||||
reacquire_conf: float = 0.18,
|
||||
lock_iou: float = 0.08,
|
||||
min_conf: float = 0.12,
|
||||
min_area: float = 4000.0,
|
||||
) -> Optional[Detection]:
|
||||
"""让当前工件在快速运动下也能被持续跟踪。
|
||||
|
||||
用一个较宽的匹配门限去和**预测出的**上一帧检测框做匹配。如果 IoU 不达标、
|
||||
但 YOLO 仍以高于 min_conf 的置信度报出了目标，就保留该检测 —— 不要返回
|
||||
None（那正是运动过程中目标丢失的主要来源）。
|
||||
过小的框（logo / 通止规尖）会被拒掉，避免机械臂锁到垫子上。
|
||||
"""
|
||||
if not dets:
|
||||
return None
|
||||
# 已锁定的跟踪在运动（模糊）时可能掉到 min_conf 以下；但新锁定不允许这样。
|
||||
floor = max(0.06, 0.50 * float(min_conf))
|
||||
usable = [d for d in dets if float(d.conf) >= floor and float(d.area) >= float(min_area)]
|
||||
if not usable:
|
||||
return None
|
||||
|
||||
locked: List[Detection] = []
|
||||
if prev_xyxy is not None:
|
||||
pw = max(8.0, float(prev_xyxy[2] - prev_xyxy[0]))
|
||||
ph = max(8.0, float(prev_xyxy[3] - prev_xyxy[1]))
|
||||
gate = max(0.90 * float(max_track_px), 1.25 * float(np.hypot(pw, ph)))
|
||||
pcx = 0.5 * (float(prev_xyxy[0]) + float(prev_xyxy[2]))
|
||||
pcy = 0.5 * (float(prev_xyxy[1]) + float(prev_xyxy[3]))
|
||||
for d in usable:
|
||||
iou = _iou_xyxy(d.xyxy, prev_xyxy)
|
||||
dpx = float(np.hypot(d.cx - pcx, d.cy - pcy))
|
||||
if iou >= float(lock_iou) or dpx <= gate:
|
||||
locked.append(d)
|
||||
|
||||
if locked:
|
||||
return max(
|
||||
locked,
|
||||
key=lambda d: (float(d.conf) * (float(d.area) ** 0.25), float(d.conf)),
|
||||
)
|
||||
|
||||
def score_new(d: Detection) -> float:
|
||||
s = float(d.conf) * 10.0 + 0.015 * np.sqrt(max(float(d.area), 1.0))
|
||||
if prefer_center is not None:
|
||||
cx0, cy0 = float(prefer_center[0]), float(prefer_center[1])
|
||||
s -= 1e-6 * ((d.cx - cx0) ** 2 + (d.cy - cy0) ** 2)
|
||||
if prev_uv is not None:
|
||||
dpx = float(np.hypot(d.cx - float(prev_uv[0]), d.cy - float(prev_uv[1])))
|
||||
if dpx <= max_track_px:
|
||||
s += 2.0 * (1.0 - dpx / max(max_track_px, 1.0))
|
||||
return s
|
||||
|
||||
best = max(usable, key=score_new)
|
||||
# 快速移动：即使 YOLO 报出的框离上一帧很远，也保留这个实时检测。
|
||||
need = max(float(min_conf), float(reacquire_conf))
|
||||
if float(best.conf) >= need:
|
||||
return best
|
||||
return None
|
||||
@@ -0,0 +1,5 @@
|
||||
# Application-level waypoints / task parameters
|
||||
demo:
|
||||
poses: [home, ready, inspect, home]
|
||||
velocity_scale: 0.5
|
||||
dwell_sec: 1.0
|
||||
@@ -0,0 +1,25 @@
|
||||
# DualArmGripperManager(load_dual_arm_gripper_config(path)) 的可选兜底配置。
|
||||
# 日常使用通过 load_from_site() 读取 ~/ros2/config/site.yaml。
|
||||
left_arm:
|
||||
robot_sn: "Rizon4-063989"
|
||||
gripper_port: "/dev/ttyUSB0"
|
||||
gripper_slave_id: 1
|
||||
gripper_baudrate: 115200
|
||||
right_arm:
|
||||
robot_sn: "Rizon4-063991"
|
||||
gripper_port: "/dev/ttyUSB1"
|
||||
gripper_slave_id: 1
|
||||
gripper_baudrate: 115200
|
||||
gripper:
|
||||
parallel:
|
||||
stroke_min: 0.0
|
||||
stroke_max: 20.0
|
||||
default_speed: 50
|
||||
default_force: 20
|
||||
auto_initialize: true
|
||||
rotary:
|
||||
auto_initialize: false
|
||||
default_speed: 50
|
||||
default_torque: 50
|
||||
finger_open_per_mille: 1000
|
||||
finger_close_per_mille: 0
|
||||
@@ -0,0 +1,47 @@
|
||||
# 工件二阶段检测配置：YOLO 定位 + HoughCircles 螺纹孔
|
||||
# 供 hole_detect_hough.py / hole_detect_node（backend=two_stage）使用
|
||||
# 操作入口：bash ~/ros2/scripts/run_hole_detect.sh --arm 89|91 --preview
|
||||
|
||||
# ----- YOLO -----
|
||||
conf: 0.25
|
||||
iou: 0.7
|
||||
imgsz: 640
|
||||
|
||||
# ----- 居中门控：bbox 中心接近画面中心才做二阶圆孔检测 -----
|
||||
# 精对准 k1/k2 时 stage2 会 skip 此门控（对准目标孔后工件必然离开画面中心）
|
||||
center_gate_enable: true
|
||||
center_max_offset_ratio_x: 0.15 # |bbox_cx - 图像中心x| <= 该比例 * 宽
|
||||
center_max_offset_ratio_y: 0.18 # |bbox_cy - 图像中心y| <= 该比例 * 高
|
||||
center_gate_draw: true # 可视化画出允许居中区域
|
||||
refine_hole_centers: true # Hough 后再用暗孔矩细化亚像素圆心
|
||||
|
||||
# ----- ROI：bbox 向外膨胀像素，裁剪时做边界保护 -----
|
||||
bbox_padding: 25
|
||||
|
||||
# ----- ROI 预处理 -----
|
||||
gaussian_ksize: 5 # 必须为奇数
|
||||
gaussian_sigma: 1.5
|
||||
|
||||
# ----- HoughCircles（全部由此读取）-----
|
||||
hough_dp: 1.2
|
||||
hough_min_dist: 50
|
||||
hough_param1: 100
|
||||
hough_param2: 26
|
||||
hough_min_radius: 10
|
||||
hough_max_radius: 30
|
||||
|
||||
# ----- 工业校验 -----
|
||||
min_radius: 10 # 有效圆孔半径下限（滤掉噪点）
|
||||
max_radius: 30 # 有效圆孔半径上限（滤掉中间大孔）
|
||||
hole_distance_min: 150 # 两孔像素距离下限
|
||||
hole_distance_max: 700 # 两孔像素距离上限
|
||||
|
||||
# ----- 时序稳帧（抑制闪烁）-----
|
||||
stabilize_hold_frames: 15 # 连续失败多少帧后才清空结果
|
||||
stabilize_ema_alpha: 0.35 # 中心点/半径 EMA，越小越稳
|
||||
stabilize_max_jump: 60 # 单帧跳变超过该像素则忽略（防闪）
|
||||
|
||||
# ----- 螺纹嵌件对（亮环签名 + 关于中心对称，深度自适应）-----
|
||||
insert_pair_enable: true
|
||||
insert_percentile: 96
|
||||
insert_bolt_radius_mm: 29.0 # 两目标孔物理间距~58mm → 螺栓圆半径29mm；按深度换算像素，尺度不变
|
||||
@@ -0,0 +1,37 @@
|
||||
# 孔定位检测配置（改 conf / 内参 / 双孔间距即可，不必改代码）
|
||||
# 权重来源：优先 site.yaml（hole_detect.weights / paths.hole_weights）；
|
||||
# 两者都缺时兜底到 models/yolo/gongjian0929.onnx。
|
||||
# 原先这里注释写的是 best.pt —— 该文件不存在，已修正。
|
||||
# backend: yolo | hough | two_stage（two_stage 请用 hole_detect.yaml）
|
||||
|
||||
model_path: ""
|
||||
backend: yolo # yolo | faster_rcnn | hough
|
||||
num_classes: 1 # 单类孔=1；左右孔分两类=2
|
||||
expected_holes: 2
|
||||
min_hole_sep_px: 20
|
||||
max_hole_sep_px: 2000
|
||||
|
||||
conf: 0.25
|
||||
iou: 0.7
|
||||
imgsz: 640
|
||||
|
||||
gaussian_ksize: 5
|
||||
gaussian_sigma: 1.5
|
||||
hough_dp: 1.2
|
||||
hough_min_dist: 40
|
||||
hough_param1: 100
|
||||
hough_param2: 28
|
||||
hough_min_radius: 6
|
||||
hough_max_radius: 45
|
||||
|
||||
stabilize_window: 7
|
||||
stabilize_ema: 0.35
|
||||
stabilize_max_jump_px: 60
|
||||
|
||||
camera_intrinsics:
|
||||
fx: 425.0
|
||||
fy: 425.0
|
||||
cx: 640.0
|
||||
cy: 360.0
|
||||
|
||||
default_depth_m: 0.10
|
||||
@@ -0,0 +1,46 @@
|
||||
# perception_node 的默认值。launch 会用 ~/ros2/config/site.yaml 的 yolo: / paths: 覆盖。
|
||||
# 日常调参**不要改这里**，改 site.yaml。
|
||||
#
|
||||
# 注意：下面 weights / conf / iou / imgsz / device / half_precision / min_conf 这几项
|
||||
# 一定被 site.yaml 覆盖，这里只是"没有 site.yaml 时的兜底"。所以它们必须和 site.yaml
|
||||
# 保持一致 —— 曾经这里写死 imgsz=416 而实际模型是 640，一旦覆盖失效就会用错尺寸。
|
||||
|
||||
perception_node:
|
||||
ros__parameters:
|
||||
camera_source: "d405"
|
||||
camera_namespace: ""
|
||||
image_topic: ""
|
||||
camera_info_topic: ""
|
||||
depth_topic: "/d405/aligned_depth_to_color/image_raw"
|
||||
weights: "models/yolo/gongjian0929.onnx"
|
||||
conf: 0.10
|
||||
iou: 0.45
|
||||
imgsz: 640
|
||||
device: "0"
|
||||
half_precision: true
|
||||
min_conf: 0.12
|
||||
vision_hz: 20.0
|
||||
z_ref_m: 0.30
|
||||
# 大跳变门限 + 重新捕获：快速运动绝不能把跟踪卡死
|
||||
jump_px: 280.0
|
||||
jump_z_m: 0.20
|
||||
min_depth_m: 0.05
|
||||
max_depth_m: 2.50
|
||||
object_frame: "tracked_object"
|
||||
camera_frame: "d405_color_optical_frame"
|
||||
# 灵敏的 KF + 向原始观测融合
|
||||
kf_q_pos: 0.002
|
||||
kf_q_vel: 0.05
|
||||
kf_r_pos: 0.0008
|
||||
meas_blend: 0.65
|
||||
# 更快的推理 + 更紧的框跟随
|
||||
infer_max_width: 640
|
||||
box_ema_alpha: 0.85
|
||||
miss_clear_frames: 2
|
||||
# 第 4 项 — 自适应 Z 深度低通滤波
|
||||
z_alpha_min: 0.10
|
||||
z_alpha_max: 0.60
|
||||
z_alpha_gain: 2.0
|
||||
feature_mode: "plane" # stereo 为 3D 质心；box = 单目框中心的射线
|
||||
plane_inlier_m: 0.008
|
||||
plane_band_m: 0.06
|
||||
@@ -0,0 +1,69 @@
|
||||
# servo_node 的默认值。launch 会用 ~/ros2/config/site.yaml 的 servo: 覆盖。
|
||||
# 日常调参不要改这个文件。
|
||||
|
||||
servo_node:
|
||||
ros__parameters:
|
||||
joint_prefix: "Rizon4-063989_"
|
||||
joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
|
||||
base_frame: "Rizon4-063989_base_link"
|
||||
ee_frame: "Rizon4-063989_flange"
|
||||
camera_frame: "d405_color_optical_frame"
|
||||
traj_topic: "rizon_arm_controller/joint_trajectory"
|
||||
point_topic: "visual_servo/filtered_point"
|
||||
handeye_file: "src/arm_application/config/handeye_result.yaml"
|
||||
use_static_handeye_tf: false
|
||||
handeye_child_frame: "d405_color_optical_frame"
|
||||
|
||||
control_hz: 100.0
|
||||
z_ref_m: 0.30
|
||||
enable_control: false
|
||||
max_extrapolate_sec: 0.08
|
||||
|
||||
# 第 1 项 — 丢失目标硬停
|
||||
watchdog_timeout_sec: 0.3
|
||||
lost_timeout_sec: 0.3
|
||||
|
||||
kp_xy: 2.00
|
||||
ki_xy: 0.02
|
||||
kd_xy: 0.50
|
||||
kp_z: 1.20
|
||||
ki_z: 0.02
|
||||
kd_z: 0.45
|
||||
i_limit_xy: 0.03
|
||||
i_limit_z: 0.025
|
||||
# 第 2 项 — 积分分离
|
||||
i_sep_pos_m: 0.005
|
||||
i_sep_ang_rad: 0.02
|
||||
|
||||
deadzone_m: 0.0015
|
||||
deadzone_z_m: 0.006
|
||||
v_max_xy: 0.15
|
||||
v_max_z: 0.07
|
||||
a_max: 0.70
|
||||
|
||||
# 第 3 项 — 速度低通滤波
|
||||
vel_lpf_alpha: 0.40
|
||||
wz_lpf_alpha: 0.40
|
||||
|
||||
# 第 5 项 — dq 滑动窗口
|
||||
dq_window_size: 5
|
||||
|
||||
min_ee_z_m: 0.20
|
||||
workspace_xyz_min: [0.08, -0.55, 0.18]
|
||||
workspace_xyz_max: [0.85, 0.55, 0.65]
|
||||
ee_x_m: 0.25
|
||||
ee_y_m: 0.00
|
||||
ee_z_m: 0.25
|
||||
ee_hold_xy: false
|
||||
ee_vertical_down: true
|
||||
ee_hold_kp: 2.0
|
||||
ee_w_max: 0.60
|
||||
damping: 0.08
|
||||
max_qdot: 0.85
|
||||
exec_horizon_sec: 0.20
|
||||
traj_stream_hz: 15.0
|
||||
min_cmd_delta_rad: 0.004
|
||||
camera_fx: 390.0
|
||||
camera_fy: 390.0
|
||||
camera_cx: 320.0
|
||||
camera_cy: 240.0
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user