修掉两个"存在但跑不起来"的脚本：CRLF 行尾 + 新增 .gitattributes 统一为 LF
症状（没有报错，只有"文件在、文档说能用、一跑就死"）
$ bash scripts/run_hole_detect.sh --arm 89 --preview
scripts/run_hole_detect.sh: 行 11: $'\r': 未找到命令
scripts/run_hole_detect.sh: 第 12 行： set: pipefail: 无效的选项名
另一处 $'\r': 未找到命令 来自 scripts/run_dual_gripper.sh（同样症状）。
根因
这两个 .sh 是 100% CRLF 行尾。bash 会把行尾的 \r 当成标识符的一部分，
于是 `case "$1" in\r`、`set -eo pipefail\r` 全部变成非法记号。
而文档在五处告诉用户跑它：README.md、docs/WORKSPACE.md、docs/ARCHITECTURE.md、
docs/SITE_CONFIG.md、docs/HOLE_DETECT.md（HOLE_DETECT.md 直接给了命令）。
即"文档齐全、文件存在、命令一敲就死"，报错信息里的那个 \r 又完全看不出来。
为什么之前没人发现（这是本次真正的教训）
bash -n 会漏报：对 run_dual_gripper.sh 它返回 OK，可实际执行立刻死在第 6 行。
所以"静态检查通过"不能当成"脚本能用"。验脚本要么真跑 --help，要么用
file/bat 看行尾。这条已写进 .gitattributes 的注释里。
改了什么
1. scripts/run_hole_detect.sh、scripts/run_dual_gripper.sh 由 CRLF 改为 LF。
去行尾符后与改前逐字节相同 —— 只动行尾，一个字符内容都没动。
已验证：两个脚本 bash -n 通过，且 run_hole_detect.sh --help 能正常打印中文帮助。
2. 新增 .gitattributes：
* text=auto eol=lf —— 本仓库自己的文件一律 LF
/src/flexiv_*/** -text —— 厂商上游代码内容与行尾都不动（R4.4）
/thirdparty/** -text
*.onnx/*.engine/*.trt 等二进制显式标 binary，避免被当成文本转换
3. 顺带把本仓库其余 19 个仍是 CRLF 的文件（.py 9 个、.yaml 2 个、.launch.py 2 个、
.msg 5 个、.srv 1 个）统一为 LF。这些文件在 Python/YAML 下本来能正常读，
所以此前没有症状；但同一仓库混着两种行尾，下次再有一个 .sh 中招就是必然。
已核对：去掉行尾符后与 HEAD 逐字节相同 —— 内容零变化。
验收
21 个文件：git diff --ignore-cr-at-eol -w 只列出 .gitattributes（零内容变化）
scripts/*.sh scripts/lib/*.sh：bash -n 失败数 0（改前是 2）
单测：608 passed，与改动前一致
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -0,0 +1,37 @@
|
|||||||
|
# =============================================================================
|
||||||
|
# .gitattributes —— 行尾统一
|
||||||
|
#
|
||||||
|
# 为什么要专门写这个文件：
|
||||||
|
# ``scripts/run_hole_detect.sh`` 和 ``scripts/run_dual_gripper.sh`` 曾经是 CRLF 行尾，
|
||||||
|
# 结果是**文件在、文档说能用、一跑就死** —— bash 在
|
||||||
|
# case "$1" in^M
|
||||||
|
# 处直接报 "未预期的记号"，而报错信息里那个 ``\r`` 是看不出来的。
|
||||||
|
# 更坑的是 ``bash -n`` 对其中一个还报 OK，只有真的执行才暴露。
|
||||||
|
# 这类"存在但不工作"的死文件，和本仓库最忌讳的静默失效是同一类问题。
|
||||||
|
#
|
||||||
|
# 规矩：
|
||||||
|
# 1. 本仓库自己的文件（scripts/、 src/arm_*/、 docs/、 config/、 handeye/、 devices/、
|
||||||
|
# models/）一律 LF，工作区也是 LF。
|
||||||
|
# 2. 厂商上游代码（src/flexiv_*/）与 thirdparty/ 一律不动 —— 见
|
||||||
|
# docs/FRAMEWORK_RULES.md 的 R4.4：上游文件不许改，改了会在下次拉取时被覆盖。
|
||||||
|
# =============================================================================
|
||||||
|
|
||||||
|
# 默认：文本文件在仓库里和工作区里都存成 LF
|
||||||
|
* text=auto eol=lf
|
||||||
|
|
||||||
|
# 厂商上游 / 第三方：内容与行尾一律保持原样（不要"顺手统一"）
|
||||||
|
/src/flexiv_*/** -text
|
||||||
|
/thirdparty/** -text
|
||||||
|
/vendor/** -text
|
||||||
|
|
||||||
|
# 二进制资产：明确不当文本处理，避免被行尾转换损坏
|
||||||
|
*.onnx binary
|
||||||
|
*.engine binary
|
||||||
|
*.plan binary
|
||||||
|
*.trt binary
|
||||||
|
*.pt binary
|
||||||
|
*.pth binary
|
||||||
|
*.png binary
|
||||||
|
*.jpg binary
|
||||||
|
*.stl binary
|
||||||
|
*.dae binary
|
||||||
@@ -1,35 +1,35 @@
|
|||||||
# 入孔检测权重
|
# 入孔检测权重
|
||||||
|
|
||||||
> **改这个目录之前先读 [`../README.md`](../README.md)**（权重模块的规则）。
|
> **改这个目录之前先读 [`../README.md`](../README.md)**（权重模块的规则）。
|
||||||
|
|
||||||
用于"把孔当框来检测"的模型（`hole_detect.backend: yolo`）。
|
用于"把孔当框来检测"的模型（`hole_detect.backend: yolo`）。
|
||||||
当前现场用的是 `two_stage`（两阶段）后端，它用的是**工件**权重
|
当前现场用的是 `two_stage`（两阶段）后端，它用的是**工件**权重
|
||||||
（`hole_detect.workpiece_weights`，指向 `models/yolo/` 里的模型），
|
（`hole_detect.workpiece_weights`，指向 `models/yolo/` 里的模型），
|
||||||
本目录的模型并不是每次都会用到。
|
本目录的模型并不是每次都会用到。
|
||||||
|
|
||||||
## 现状
|
## 现状
|
||||||
|
|
||||||
- `detect_best_1.onnx` —— 被 `hole_detect.workpiece_weights` 使用（`two_stage` 后端）。
|
- `detect_best_1.onnx` —— 被 `hole_detect.workpiece_weights` 使用（`two_stage` 后端）。
|
||||||
这是本目录唯一在用的模型。
|
这是本目录唯一在用的模型。
|
||||||
|
|
||||||
## 已修掉的悬空引用
|
## 已修掉的悬空引用
|
||||||
|
|
||||||
`config/site.yaml` 的 `paths.hole_weights` 与 `hole_detect.weights` 原先都指向
|
`config/site.yaml` 的 `paths.hole_weights` 与 `hole_detect.weights` 原先都指向
|
||||||
**不存在**的文件 `models/hole_insert/best.pt`。`backend: two_stage` 时这两处不被读取，
|
**不存在**的文件 `models/hole_insert/best.pt`。`backend: two_stage` 时这两处不被读取，
|
||||||
所以一直没暴露；一旦切成 `backend: yolo` 就会启动失败。
|
所以一直没暴露；一旦切成 `backend: yolo` 就会启动失败。
|
||||||
|
|
||||||
现已改为实际存在的 `detect_best_1.onnx`。同一个悬空的 `best.pt` 还藏在代码里
|
现已改为实际存在的 `detect_best_1.onnx`。同一个悬空的 `best.pt` 还藏在代码里
|
||||||
（`hole_detector.py` 的 `weights_from_site` / `_default_model_path`，那里三个候选
|
（`hole_detector.py` 的 `weights_from_site` / `_default_model_path`，那里三个候选
|
||||||
路径全都不存在，函数却总会返回第一个），也一并修成了以 site.yaml 为准、
|
路径全都不存在，函数却总会返回第一个），也一并修成了以 site.yaml 为准、
|
||||||
再退到实际存在的模型。
|
再退到实际存在的模型。
|
||||||
|
|
||||||
改权重相关路径后，用 `bash ~/ros2/scripts/weights.sh doctor` 复查悬空引用。
|
改权重相关路径后，用 `bash ~/ros2/scripts/weights.sh doctor` 复查悬空引用。
|
||||||
|
|
||||||
## 加新模型 / 看标签
|
## 加新模型 / 看标签
|
||||||
|
|
||||||
和 `models/yolo/` 完全一样，只是分组名换成 `hole_insert`：
|
和 `models/yolo/` 完全一样，只是分组名换成 `hole_insert`：
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
bash ~/ros2/scripts/weights.sh convert staging/xxx.onnx --name hole_detect --group hole_insert
|
bash ~/ros2/scripts/weights.sh convert staging/xxx.onnx --name hole_detect --group hole_insert
|
||||||
bash ~/ros2/scripts/weights.sh labels detect_best_1 --group hole_insert
|
bash ~/ros2/scripts/weights.sh labels detect_best_1 --group hole_insert
|
||||||
```
|
```
|
||||||
|
|||||||
+25
-25
@@ -1,25 +1,25 @@
|
|||||||
#!/usr/bin/env bash
|
#!/usr/bin/env bash
|
||||||
# 把两个 DH 夹爪（左 PGEA + 右 RGI）作为同一个 ROS 2 节点启动。
|
# 把两个 DH 夹爪（左 PGEA + 右 RGI）作为同一个 ROS 2 节点启动。
|
||||||
# bash ~/ros2/scripts/run_dual_gripper.sh
|
# bash ~/ros2/scripts/run_dual_gripper.sh
|
||||||
# 话题：/dual_gripper_driver/left|right/{command,status}
|
# 话题：/dual_gripper_driver/left|right/{command,status}
|
||||||
# 服务：/dual_gripper_driver/left|right/execute (arm_msgs/srv/GripperExecute)
|
# 服务：/dual_gripper_driver/left|right/execute (arm_msgs/srv/GripperExecute)
|
||||||
|
|
||||||
set -eo pipefail
|
set -eo pipefail
|
||||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||||
|
|
||||||
# shellcheck disable=SC1091
|
# shellcheck disable=SC1091
|
||||||
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||||||
parse_arm_args "$@"
|
parse_arm_args "$@"
|
||||||
set -- "${PARSE_ARM_REST[@]}"
|
set -- "${PARSE_ARM_REST[@]}"
|
||||||
|
|
||||||
# shellcheck disable=SC1091
|
# shellcheck disable=SC1091
|
||||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||||
# shellcheck disable=SC1091
|
# shellcheck disable=SC1091
|
||||||
source "${ROOT}/scripts/env.sh"
|
source "${ROOT}/scripts/env.sh"
|
||||||
|
|
||||||
pkill -f 'arm_application/dual_gripper_node.py' 2>/dev/null || true
|
pkill -f 'arm_application/dual_gripper_node.py' 2>/dev/null || true
|
||||||
pkill -f 'lib/arm_application/dual_gripper' 2>/dev/null || true
|
pkill -f 'lib/arm_application/dual_gripper' 2>/dev/null || true
|
||||||
sleep 0.3
|
sleep 0.3
|
||||||
|
|
||||||
echo "=== dual gripper PGEA ${SITE_GRIP_PORT:-/dev/ttyUSB0} + RGI (site.yaml robots[]) ==="
|
echo "=== dual gripper PGEA ${SITE_GRIP_PORT:-/dev/ttyUSB0} + RGI (site.yaml robots[]) ==="
|
||||||
exec ros2 launch arm_application dual_gripper.launch.py sides:=left,right allow_offline:=true
|
exec ros2 launch arm_application dual_gripper.launch.py sides:=left,right allow_offline:=true
|
||||||
|
|||||||
+42
-42
@@ -1,42 +1,42 @@
|
|||||||
#!/usr/bin/env bash
|
#!/usr/bin/env bash
|
||||||
# 用当前臂腕部 D405 做孔检测。只检测 —— 不做插入动作。
|
# 用当前臂腕部 D405 做孔检测。只检测 —— 不做插入动作。
|
||||||
#
|
#
|
||||||
# bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
|
# bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
|
||||||
# bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
|
# bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
|
||||||
#
|
#
|
||||||
# 后端取自 site.yaml 的 hole_detect.backend:
|
# 后端取自 site.yaml 的 hole_detect.backend:
|
||||||
# two_stage YOLO 工件 bbox + HoughCircles（camera_test 的 hole_detect.py）
|
# two_stage YOLO 工件 bbox + HoughCircles（camera_test 的 hole_detect.py）
|
||||||
# yolo 把 YOLO 框直接当作孔
|
# yolo 把 YOLO 框直接当作孔
|
||||||
# hough 整图 HoughCircles
|
# hough 整图 HoughCircles
|
||||||
|
|
||||||
set -eo pipefail
|
set -eo pipefail
|
||||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||||
PREVIEW=false
|
PREVIEW=false
|
||||||
|
|
||||||
# shellcheck disable=SC1091
|
# shellcheck disable=SC1091
|
||||||
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||||||
parse_arm_args "$@"
|
parse_arm_args "$@"
|
||||||
set -- "${PARSE_ARM_REST[@]}"
|
set -- "${PARSE_ARM_REST[@]}"
|
||||||
|
|
||||||
while [[ $# -gt 0 ]]; do
|
while [[ $# -gt 0 ]]; do
|
||||||
case "$1" in
|
case "$1" in
|
||||||
--preview) PREVIEW=true; shift ;;
|
--preview) PREVIEW=true; shift ;;
|
||||||
-h|--help) sed -n '2,16p' "$0"; exit 0 ;;
|
-h|--help) sed -n '2,16p' "$0"; exit 0 ;;
|
||||||
*) echo "Unknown: $1" >&2; exit 1 ;;
|
*) echo "Unknown: $1" >&2; exit 1 ;;
|
||||||
esac
|
esac
|
||||||
done
|
done
|
||||||
|
|
||||||
# shellcheck disable=SC1091
|
# shellcheck disable=SC1091
|
||||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||||
# shellcheck disable=SC1091
|
# shellcheck disable=SC1091
|
||||||
source "${ROOT}/scripts/env.sh"
|
source "${ROOT}/scripts/env.sh"
|
||||||
|
|
||||||
pkill -f 'arm_application/hole_detect_node.py' 2>/dev/null || true
|
pkill -f 'arm_application/hole_detect_node.py' 2>/dev/null || true
|
||||||
pkill -f 'lib/arm_application/hole_detect' 2>/dev/null || true
|
pkill -f 'lib/arm_application/hole_detect' 2>/dev/null || true
|
||||||
sleep 0.2
|
sleep 0.2
|
||||||
|
|
||||||
BACKEND="${SITE_HOLE_BACKEND:-two_stage}"
|
BACKEND="${SITE_HOLE_BACKEND:-two_stage}"
|
||||||
echo "=== hole_detect arm=${SITE_ROBOT_ARM:-?} cam=${SITE_CAM_NAME} backend=${BACKEND} preview=${PREVIEW} ==="
|
echo "=== hole_detect arm=${SITE_ROBOT_ARM:-?} cam=${SITE_CAM_NAME} backend=${BACKEND} preview=${PREVIEW} ==="
|
||||||
exec ros2 launch arm_application hole_detect.launch.py \
|
exec ros2 launch arm_application hole_detect.launch.py \
|
||||||
backend:="${BACKEND}" \
|
backend:="${BACKEND}" \
|
||||||
preview:="${PREVIEW}"
|
preview:="${PREVIEW}"
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -1,142 +1,142 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""DH PGEA（左臂）与 RGI（右臂）的命令行工具。从 site.yaml 读取串口。"""
|
"""DH PGEA（左臂）与 RGI（右臂）的命令行工具。从 site.yaml 读取串口。"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import argparse
|
import argparse
|
||||||
import os
|
import os
|
||||||
import sys
|
import sys
|
||||||
import time
|
import time
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
|
||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
from arm_application.dual_gripper import DualArmGripperManager, load_from_site
|
from arm_application.dual_gripper import DualArmGripperManager, load_from_site
|
||||||
|
|
||||||
|
|
||||||
def _site_path() -> Path:
|
def _site_path() -> Path:
|
||||||
env = os.environ.get("SITE_CONFIG", "").strip()
|
env = os.environ.get("SITE_CONFIG", "").strip()
|
||||||
if env:
|
if env:
|
||||||
return Path(env).expanduser()
|
return Path(env).expanduser()
|
||||||
return Path.home() / "ros2" / "config" / "site.yaml"
|
return Path.home() / "ros2" / "config" / "site.yaml"
|
||||||
|
|
||||||
|
|
||||||
def _print_status(label: str, status) -> None:
|
def _print_status(label: str, status) -> None:
|
||||||
print(
|
print(
|
||||||
f" {label}: state={status.state.name} "
|
f" {label}: state={status.state.name} "
|
||||||
f"pos={status.position:.2f} finger={status.finger_position_per_mille:.0f}‰ "
|
f"pos={status.position:.2f} finger={status.finger_position_per_mille:.0f}‰ "
|
||||||
f"rot={status.rotation_deg:.2f}° moving={status.is_moving} "
|
f"rot={status.rotation_deg:.2f}° moving={status.is_moving} "
|
||||||
f"gripped={status.is_gripped} init={status.initialized} "
|
f"gripped={status.is_gripped} init={status.initialized} "
|
||||||
f"err={status.error_msg or '-'}"
|
f"err={status.error_msg or '-'}"
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
def _normalize_arm(raw: str) -> str:
|
def _normalize_arm(raw: str) -> str:
|
||||||
t = (raw or "").strip().lower()
|
t = (raw or "").strip().lower()
|
||||||
if t in ("89", "left", "l", "063989"):
|
if t in ("89", "left", "l", "063989"):
|
||||||
return "left"
|
return "left"
|
||||||
if t in ("91", "right", "r", "063991"):
|
if t in ("91", "right", "r", "063991"):
|
||||||
return "right"
|
return "right"
|
||||||
if t in ("both", "dual", "all"):
|
if t in ("both", "dual", "all"):
|
||||||
return "both"
|
return "both"
|
||||||
raise SystemExit(f"未知的 --arm {raw!r}（可用 89|91|left|right|both）")
|
raise SystemExit(f"未知的 --arm {raw!r}（可用 89|91|left|right|both）")
|
||||||
|
|
||||||
|
|
||||||
def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
|
def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
|
||||||
gripper = manager.left
|
gripper = manager.left
|
||||||
print(f"=== PGEA 平行夹爪（{gripper.cfg.port}）===")
|
print(f"=== PGEA 平行夹爪（{gripper.cfg.port}）===")
|
||||||
gripper.connect()
|
gripper.connect()
|
||||||
_print_status("已连接", gripper.get_status())
|
_print_status("已连接", gripper.get_status())
|
||||||
if args.test in ("init", "probe", "all"):
|
if args.test in ("init", "probe", "all"):
|
||||||
if args.test != "probe":
|
if args.test != "probe":
|
||||||
gripper.initialize(full=args.init_full)
|
gripper.initialize(full=args.init_full)
|
||||||
_print_status("初始化", gripper.get_status())
|
_print_status("初始化", gripper.get_status())
|
||||||
if args.test in ("open", "all"):
|
if args.test in ("open", "all"):
|
||||||
_print_status("张开", gripper.open(force=args.force, speed=args.speed))
|
_print_status("张开", gripper.open(force=args.force, speed=args.speed))
|
||||||
if args.test in ("close", "all"):
|
if args.test in ("close", "all"):
|
||||||
_print_status("闭合", gripper.close(force=args.force, speed=args.speed))
|
_print_status("闭合", gripper.close(force=args.force, speed=args.speed))
|
||||||
if args.test in ("move", "pos") and args.pos is not None:
|
if args.test in ("move", "pos") and args.pos is not None:
|
||||||
_print_status("移动", gripper.move_to(args.pos, force=args.force, speed=args.speed))
|
_print_status("移动", gripper.move_to(args.pos, force=args.force, speed=args.speed))
|
||||||
if args.test in ("status", "probe"):
|
if args.test in ("status", "probe"):
|
||||||
_print_status("状态", gripper.get_status())
|
_print_status("状态", gripper.get_status())
|
||||||
|
|
||||||
|
|
||||||
def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
|
def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
|
||||||
gripper = manager.right
|
gripper = manager.right
|
||||||
print(f"=== RGI 旋转夹爪（{gripper.cfg.port}）===")
|
print(f"=== RGI 旋转夹爪（{gripper.cfg.port}）===")
|
||||||
gripper.connect()
|
gripper.connect()
|
||||||
_print_status("已连接", gripper.get_status())
|
_print_status("已连接", gripper.get_status())
|
||||||
if args.test in ("init", "all"):
|
if args.test in ("init", "all"):
|
||||||
gripper.initialize(full=args.init_full)
|
gripper.initialize(full=args.init_full)
|
||||||
_print_status("初始化", gripper.get_status())
|
_print_status("初始化", gripper.get_status())
|
||||||
if args.test in ("open", "all"):
|
if args.test in ("open", "all"):
|
||||||
_print_status("张开", gripper.open_fingers(force=args.force, speed=args.speed))
|
_print_status("张开", gripper.open_fingers(force=args.force, speed=args.speed))
|
||||||
if args.test in ("close", "all"):
|
if args.test in ("close", "all"):
|
||||||
_print_status("闭合", gripper.close_fingers(force=args.force, speed=args.speed))
|
_print_status("闭合", gripper.close_fingers(force=args.force, speed=args.speed))
|
||||||
if args.test in ("move", "pos") and args.pos is not None:
|
if args.test in ("move", "pos") and args.pos is not None:
|
||||||
_print_status("手指", gripper.move_fingers(int(args.pos), force=args.force, speed=args.speed))
|
_print_status("手指", gripper.move_fingers(int(args.pos), force=args.force, speed=args.speed))
|
||||||
if args.test in ("rotate", "rotate_to", "all"):
|
if args.test in ("rotate", "rotate_to", "all"):
|
||||||
if args.revolutions is not None:
|
if args.revolutions is not None:
|
||||||
_print_status(
|
_print_status(
|
||||||
"圈数",
|
"圈数",
|
||||||
gripper.rotate_revolutions(args.revolutions, speed=args.speed, torque=args.force),
|
gripper.rotate_revolutions(args.revolutions, speed=args.speed, torque=args.force),
|
||||||
)
|
)
|
||||||
else:
|
else:
|
||||||
angle = args.angle if args.angle is not None else 90.0
|
angle = args.angle if args.angle is not None else 90.0
|
||||||
_print_status("旋转", gripper.rotate_to(angle, speed=args.speed, torque=args.force))
|
_print_status("旋转", gripper.rotate_to(angle, speed=args.speed, torque=args.force))
|
||||||
if args.test == "rotate_by":
|
if args.test == "rotate_by":
|
||||||
delta = args.angle if args.angle is not None else 45.0
|
delta = args.angle if args.angle is not None else 45.0
|
||||||
_print_status("相对旋转", gripper.rotate_by(delta, speed=args.speed, torque=args.force))
|
_print_status("相对旋转", gripper.rotate_by(delta, speed=args.speed, torque=args.force))
|
||||||
if args.test in ("status", "probe"):
|
if args.test in ("status", "probe"):
|
||||||
_print_status("状态", gripper.get_status())
|
_print_status("状态", gripper.get_status())
|
||||||
|
|
||||||
|
|
||||||
def main() -> int:
|
def main() -> int:
|
||||||
parser = argparse.ArgumentParser(description="DH PGEA / RGI 命令行工具（串口来自 site.yaml）")
|
parser = argparse.ArgumentParser(description="DH PGEA / RGI 命令行工具（串口来自 site.yaml）")
|
||||||
parser.add_argument("--arm", default=os.environ.get("ROBOT_ARM") or os.environ.get("ROBOT_SN") or "89")
|
parser.add_argument("--arm", default=os.environ.get("ROBOT_ARM") or os.environ.get("ROBOT_SN") or "89")
|
||||||
parser.add_argument(
|
parser.add_argument(
|
||||||
"--test",
|
"--test",
|
||||||
default="status",
|
default="status",
|
||||||
help="probe|init|open|close|move|pos|rotate|rotate_by|status",
|
help="probe|init|open|close|move|pos|rotate|rotate_by|status",
|
||||||
)
|
)
|
||||||
parser.add_argument("--pos", type=float, default=None, help="PGEA 为 mm；RGI 为手指 0-1000")
|
parser.add_argument("--pos", type=float, default=None, help="PGEA 为 mm；RGI 为手指 0-1000")
|
||||||
parser.add_argument("--angle", type=float, default=None)
|
parser.add_argument("--angle", type=float, default=None)
|
||||||
parser.add_argument("--revolutions", type=float, default=None)
|
parser.add_argument("--revolutions", type=float, default=None)
|
||||||
parser.add_argument("--speed", type=int, default=25)
|
parser.add_argument("--speed", type=int, default=25)
|
||||||
parser.add_argument("--force", type=int, default=33)
|
parser.add_argument("--force", type=int, default=33)
|
||||||
parser.add_argument("--init-full", action="store_true")
|
parser.add_argument("--init-full", action="store_true")
|
||||||
parser.add_argument("--site", default="")
|
parser.add_argument("--site", default="")
|
||||||
args = parser.parse_args()
|
args = parser.parse_args()
|
||||||
|
|
||||||
site = Path(args.site).expanduser() if args.site else _site_path()
|
site = Path(args.site).expanduser() if args.site else _site_path()
|
||||||
if not site.is_file():
|
if not site.is_file():
|
||||||
print(f"错误：site.yaml 缺失：{site}", file=sys.stderr)
|
print(f"错误：site.yaml 缺失：{site}", file=sys.stderr)
|
||||||
return 1
|
return 1
|
||||||
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||||||
manager = DualArmGripperManager(load_from_site(data))
|
manager = DualArmGripperManager(load_from_site(data))
|
||||||
arm = _normalize_arm(str(args.arm))
|
arm = _normalize_arm(str(args.arm))
|
||||||
print(f"site={site} arm={arm} test={args.test}")
|
print(f"site={site} arm={arm} test={args.test}")
|
||||||
|
|
||||||
try:
|
try:
|
||||||
if arm == "left":
|
if arm == "left":
|
||||||
test_parallel(manager, args)
|
test_parallel(manager, args)
|
||||||
elif arm == "right":
|
elif arm == "right":
|
||||||
test_rotary(manager, args)
|
test_rotary(manager, args)
|
||||||
else:
|
else:
|
||||||
test_parallel(manager, args)
|
test_parallel(manager, args)
|
||||||
test_rotary(manager, args)
|
test_rotary(manager, args)
|
||||||
except KeyboardInterrupt:
|
except KeyboardInterrupt:
|
||||||
print("收到中断 —— 停止")
|
print("收到中断 —— 停止")
|
||||||
manager.emergency_stop()
|
manager.emergency_stop()
|
||||||
return 130
|
return 130
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
print(f"错误：{exc}", file=sys.stderr)
|
print(f"错误：{exc}", file=sys.stderr)
|
||||||
return 1
|
return 1
|
||||||
finally:
|
finally:
|
||||||
manager.disconnect_all()
|
manager.disconnect_all()
|
||||||
time.sleep(0.05)
|
time.sleep(0.05)
|
||||||
return 0
|
return 0
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
raise SystemExit(main())
|
raise SystemExit(main())
|
||||||
|
|||||||
@@ -1,318 +1,318 @@
|
|||||||
"""ROS 2 双臂 DH 夹爪节点（左臂 PGEA + 右臂 RGI）。使用 arm_msgs，而非 Flexiv RDK。"""
|
"""ROS 2 双臂 DH 夹爪节点（左臂 PGEA + 右臂 RGI）。使用 arm_msgs，而非 Flexiv RDK。"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import os
|
import os
|
||||||
import threading
|
import threading
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
|
||||||
import rclpy
|
import rclpy
|
||||||
import yaml
|
import yaml
|
||||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||||
from rclpy.executors import MultiThreadedExecutor
|
from rclpy.executors import MultiThreadedExecutor
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
|
|
||||||
from arm_application.dual_gripper import (
|
from arm_application.dual_gripper import (
|
||||||
DualArmGripperManager,
|
DualArmGripperManager,
|
||||||
GripperState,
|
GripperState,
|
||||||
load_dual_arm_gripper_config,
|
load_dual_arm_gripper_config,
|
||||||
load_from_site,
|
load_from_site,
|
||||||
)
|
)
|
||||||
from arm_msgs.msg import GripperCommand as GripperCommandMsg
|
from arm_msgs.msg import GripperCommand as GripperCommandMsg
|
||||||
from arm_msgs.msg import GripperStatus as GripperStatusMsg
|
from arm_msgs.msg import GripperStatus as GripperStatusMsg
|
||||||
from arm_msgs.srv import GripperExecute
|
from arm_msgs.srv import GripperExecute
|
||||||
|
|
||||||
|
|
||||||
def _site_path() -> Path:
|
def _site_path() -> Path:
|
||||||
env = os.environ.get("SITE_CONFIG", "").strip()
|
env = os.environ.get("SITE_CONFIG", "").strip()
|
||||||
if env:
|
if env:
|
||||||
return Path(env).expanduser()
|
return Path(env).expanduser()
|
||||||
return Path.home() / "ros2" / "config" / "site.yaml"
|
return Path.home() / "ros2" / "config" / "site.yaml"
|
||||||
|
|
||||||
|
|
||||||
def _as_bool(val, default: bool = True) -> bool:
|
def _as_bool(val, default: bool = True) -> bool:
|
||||||
if val is None:
|
if val is None:
|
||||||
return default
|
return default
|
||||||
if isinstance(val, str):
|
if isinstance(val, str):
|
||||||
return val.strip().lower() in ("1", "true", "yes", "on")
|
return val.strip().lower() in ("1", "true", "yes", "on")
|
||||||
return bool(val)
|
return bool(val)
|
||||||
|
|
||||||
|
|
||||||
class DualGripperNode(Node):
|
class DualGripperNode(Node):
|
||||||
def __init__(self) -> None:
|
def __init__(self) -> None:
|
||||||
super().__init__("dual_gripper_driver")
|
super().__init__("dual_gripper_driver")
|
||||||
self.declare_parameter("config_file", "")
|
self.declare_parameter("config_file", "")
|
||||||
self.declare_parameter("auto_connect", True)
|
self.declare_parameter("auto_connect", True)
|
||||||
self.declare_parameter("allow_offline", True)
|
self.declare_parameter("allow_offline", True)
|
||||||
self.declare_parameter("publish_hz", 10.0)
|
self.declare_parameter("publish_hz", 10.0)
|
||||||
self.declare_parameter("sides", "left,right")
|
self.declare_parameter("sides", "left,right")
|
||||||
|
|
||||||
self._cb_group = ReentrantCallbackGroup()
|
self._cb_group = ReentrantCallbackGroup()
|
||||||
self._locks = {"left": threading.Lock(), "right": threading.Lock()}
|
self._locks = {"left": threading.Lock(), "right": threading.Lock()}
|
||||||
sides_raw = str(self.get_parameter("sides").value or "left,right")
|
sides_raw = str(self.get_parameter("sides").value or "left,right")
|
||||||
self._sides = [s.strip().lower() for s in sides_raw.split(",") if s.strip()]
|
self._sides = [s.strip().lower() for s in sides_raw.split(",") if s.strip()]
|
||||||
if not self._sides:
|
if not self._sides:
|
||||||
self._sides = ["left", "right"]
|
self._sides = ["left", "right"]
|
||||||
|
|
||||||
cfg_path = self.get_parameter("config_file").get_parameter_value().string_value.strip()
|
cfg_path = self.get_parameter("config_file").get_parameter_value().string_value.strip()
|
||||||
if cfg_path:
|
if cfg_path:
|
||||||
self._manager = DualArmGripperManager(load_dual_arm_gripper_config(cfg_path))
|
self._manager = DualArmGripperManager(load_dual_arm_gripper_config(cfg_path))
|
||||||
else:
|
else:
|
||||||
site = _site_path()
|
site = _site_path()
|
||||||
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||||||
self._manager = DualArmGripperManager(load_from_site(data))
|
self._manager = DualArmGripperManager(load_from_site(data))
|
||||||
self.get_logger().info(f"夹爪配置来自 site.yaml：{site}")
|
self.get_logger().info(f"夹爪配置来自 site.yaml：{site}")
|
||||||
|
|
||||||
self._connect_errors: dict[str, str] = {}
|
self._connect_errors: dict[str, str] = {}
|
||||||
if _as_bool(self.get_parameter("auto_connect").value, True):
|
if _as_bool(self.get_parameter("auto_connect").value, True):
|
||||||
self._connect_all()
|
self._connect_all()
|
||||||
|
|
||||||
self._left_pub = self.create_publisher(GripperStatusMsg, "~/left/status", 10)
|
self._left_pub = self.create_publisher(GripperStatusMsg, "~/left/status", 10)
|
||||||
self._right_pub = self.create_publisher(GripperStatusMsg, "~/right/status", 10)
|
self._right_pub = self.create_publisher(GripperStatusMsg, "~/right/status", 10)
|
||||||
if "left" in self._sides:
|
if "left" in self._sides:
|
||||||
self.create_subscription(
|
self.create_subscription(
|
||||||
GripperCommandMsg,
|
GripperCommandMsg,
|
||||||
"~/left/command",
|
"~/left/command",
|
||||||
lambda msg: self._on_command("left", msg),
|
lambda msg: self._on_command("left", msg),
|
||||||
10,
|
10,
|
||||||
callback_group=self._cb_group,
|
callback_group=self._cb_group,
|
||||||
)
|
)
|
||||||
self.create_service(
|
self.create_service(
|
||||||
GripperExecute,
|
GripperExecute,
|
||||||
"~/left/execute",
|
"~/left/execute",
|
||||||
lambda req, resp: self._on_execute("left", req, resp),
|
lambda req, resp: self._on_execute("left", req, resp),
|
||||||
callback_group=self._cb_group,
|
callback_group=self._cb_group,
|
||||||
)
|
)
|
||||||
if "right" in self._sides:
|
if "right" in self._sides:
|
||||||
self.create_subscription(
|
self.create_subscription(
|
||||||
GripperCommandMsg,
|
GripperCommandMsg,
|
||||||
"~/right/command",
|
"~/right/command",
|
||||||
lambda msg: self._on_command("right", msg),
|
lambda msg: self._on_command("right", msg),
|
||||||
10,
|
10,
|
||||||
callback_group=self._cb_group,
|
callback_group=self._cb_group,
|
||||||
)
|
)
|
||||||
self.create_service(
|
self.create_service(
|
||||||
GripperExecute,
|
GripperExecute,
|
||||||
"~/right/execute",
|
"~/right/execute",
|
||||||
lambda req, resp: self._on_execute("right", req, resp),
|
lambda req, resp: self._on_execute("right", req, resp),
|
||||||
callback_group=self._cb_group,
|
callback_group=self._cb_group,
|
||||||
)
|
)
|
||||||
|
|
||||||
period = 1.0 / max(1e-3, float(self.get_parameter("publish_hz").value))
|
period = 1.0 / max(1e-3, float(self.get_parameter("publish_hz").value))
|
||||||
self.create_timer(period, self._publish_status, callback_group=self._cb_group)
|
self.create_timer(period, self._publish_status, callback_group=self._cb_group)
|
||||||
self.get_logger().info(
|
self.get_logger().info(
|
||||||
f"dual_gripper_driver ready sides={self._sides} "
|
f"dual_gripper_driver ready sides={self._sides} "
|
||||||
"~/left|right/command status execute"
|
"~/left|right/command status execute"
|
||||||
)
|
)
|
||||||
|
|
||||||
def _connect_all(self) -> None:
|
def _connect_all(self) -> None:
|
||||||
mapping = (("left", self._manager.left), ("right", self._manager.right))
|
mapping = (("left", self._manager.left), ("right", self._manager.right))
|
||||||
for side, gripper in mapping:
|
for side, gripper in mapping:
|
||||||
if side not in self._sides:
|
if side not in self._sides:
|
||||||
continue
|
continue
|
||||||
try:
|
try:
|
||||||
gripper.connect()
|
gripper.connect()
|
||||||
self.get_logger().info(f"{side} 夹爪已连接：{gripper.cfg.port}")
|
self.get_logger().info(f"{side} 夹爪已连接：{gripper.cfg.port}")
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
self._connect_errors[side] = str(exc)
|
self._connect_errors[side] = str(exc)
|
||||||
self.get_logger().error(f"{side} 夹爪连接失败：{exc}")
|
self.get_logger().error(f"{side} 夹爪连接失败：{exc}")
|
||||||
if self._connect_errors and not _as_bool(self.get_parameter("allow_offline").value, True):
|
if self._connect_errors and not _as_bool(self.get_parameter("allow_offline").value, True):
|
||||||
raise RuntimeError(f"夹爪连接失败：{self._connect_errors}")
|
raise RuntimeError(f"夹爪连接失败：{self._connect_errors}")
|
||||||
|
|
||||||
def _gripper(self, side: str):
|
def _gripper(self, side: str):
|
||||||
return self._manager.left if side == "left" else self._manager.right
|
return self._manager.left if side == "left" else self._manager.right
|
||||||
|
|
||||||
def _execute(self, side: str, msg: GripperCommandMsg):
|
def _execute(self, side: str, msg: GripperCommandMsg):
|
||||||
gripper = self._gripper(side)
|
gripper = self._gripper(side)
|
||||||
command = (msg.command or "").strip().lower()
|
command = (msg.command or "").strip().lower()
|
||||||
timeout = float(msg.timeout or 10.0)
|
timeout = float(msg.timeout or 10.0)
|
||||||
wait = bool(msg.wait_complete)
|
wait = bool(msg.wait_complete)
|
||||||
|
|
||||||
if side in self._connect_errors and not gripper.is_connected():
|
if side in self._connect_errors and not gripper.is_connected():
|
||||||
status = gripper.get_status()
|
status = gripper.get_status()
|
||||||
status.error_msg = self._connect_errors[side]
|
status.error_msg = self._connect_errors[side]
|
||||||
return status
|
return status
|
||||||
|
|
||||||
if command == "connect":
|
if command == "connect":
|
||||||
gripper.connect()
|
gripper.connect()
|
||||||
self._connect_errors.pop(side, None)
|
self._connect_errors.pop(side, None)
|
||||||
return gripper.get_status()
|
return gripper.get_status()
|
||||||
if command == "init":
|
if command == "init":
|
||||||
gripper.initialize(full=False)
|
gripper.initialize(full=False)
|
||||||
return gripper.get_status()
|
return gripper.get_status()
|
||||||
if command == "init_full":
|
if command == "init_full":
|
||||||
gripper.initialize(full=True)
|
gripper.initialize(full=True)
|
||||||
return gripper.get_status()
|
return gripper.get_status()
|
||||||
if command == "stop":
|
if command == "stop":
|
||||||
gripper.stop()
|
gripper.stop()
|
||||||
return gripper.get_status()
|
return gripper.get_status()
|
||||||
|
|
||||||
if side == "left":
|
if side == "left":
|
||||||
if command == "open":
|
if command == "open":
|
||||||
return gripper.open(
|
return gripper.open(
|
||||||
force=int(msg.force) if msg.force else None,
|
force=int(msg.force) if msg.force else None,
|
||||||
speed=int(msg.speed) if msg.speed else None,
|
speed=int(msg.speed) if msg.speed else None,
|
||||||
wait_complete=wait,
|
wait_complete=wait,
|
||||||
timeout=timeout,
|
timeout=timeout,
|
||||||
)
|
)
|
||||||
if command == "close":
|
if command == "close":
|
||||||
return gripper.close(
|
return gripper.close(
|
||||||
force=int(msg.force) if msg.force else None,
|
force=int(msg.force) if msg.force else None,
|
||||||
speed=int(msg.speed) if msg.speed else None,
|
speed=int(msg.speed) if msg.speed else None,
|
||||||
wait_complete=wait,
|
wait_complete=wait,
|
||||||
timeout=timeout,
|
timeout=timeout,
|
||||||
)
|
)
|
||||||
if command == "move":
|
if command == "move":
|
||||||
return gripper.move_to(
|
return gripper.move_to(
|
||||||
float(msg.position),
|
float(msg.position),
|
||||||
force=int(msg.force) if msg.force else None,
|
force=int(msg.force) if msg.force else None,
|
||||||
speed=int(msg.speed) if msg.speed else None,
|
speed=int(msg.speed) if msg.speed else None,
|
||||||
wait_complete=wait,
|
wait_complete=wait,
|
||||||
timeout=timeout,
|
timeout=timeout,
|
||||||
)
|
)
|
||||||
if command == "set_force":
|
if command == "set_force":
|
||||||
gripper.set_force(int(msg.force))
|
gripper.set_force(int(msg.force))
|
||||||
return gripper.get_status()
|
return gripper.get_status()
|
||||||
if command == "set_speed":
|
if command == "set_speed":
|
||||||
gripper.set_speed(int(msg.speed))
|
gripper.set_speed(int(msg.speed))
|
||||||
return gripper.get_status()
|
return gripper.get_status()
|
||||||
else:
|
else:
|
||||||
if command == "open":
|
if command == "open":
|
||||||
return gripper.open_fingers(
|
return gripper.open_fingers(
|
||||||
force=int(msg.force) if msg.force else None,
|
force=int(msg.force) if msg.force else None,
|
||||||
speed=int(msg.speed) if msg.speed else None,
|
speed=int(msg.speed) if msg.speed else None,
|
||||||
wait_complete=wait,
|
wait_complete=wait,
|
||||||
timeout=timeout,
|
timeout=timeout,
|
||||||
)
|
)
|
||||||
if command == "close":
|
if command == "close":
|
||||||
return gripper.close_fingers(
|
return gripper.close_fingers(
|
||||||
force=int(msg.force) if msg.force else None,
|
force=int(msg.force) if msg.force else None,
|
||||||
speed=int(msg.speed) if msg.speed else None,
|
speed=int(msg.speed) if msg.speed else None,
|
||||||
wait_complete=wait,
|
wait_complete=wait,
|
||||||
timeout=timeout,
|
timeout=timeout,
|
||||||
)
|
)
|
||||||
if command == "move":
|
if command == "move":
|
||||||
return gripper.move_fingers(
|
return gripper.move_fingers(
|
||||||
int(msg.position),
|
int(msg.position),
|
||||||
force=int(msg.force) if msg.force else None,
|
force=int(msg.force) if msg.force else None,
|
||||||
speed=int(msg.speed) if msg.speed else None,
|
speed=int(msg.speed) if msg.speed else None,
|
||||||
wait_complete=wait,
|
wait_complete=wait,
|
||||||
timeout=timeout,
|
timeout=timeout,
|
||||||
)
|
)
|
||||||
if command in ("rotate", "rotate_to"):
|
if command in ("rotate", "rotate_to"):
|
||||||
return gripper.rotate_to(
|
return gripper.rotate_to(
|
||||||
float(msg.position),
|
float(msg.position),
|
||||||
speed=int(msg.speed) if msg.speed else None,
|
speed=int(msg.speed) if msg.speed else None,
|
||||||
torque=int(msg.force) if msg.force else None,
|
torque=int(msg.force) if msg.force else None,
|
||||||
wait_complete=wait,
|
wait_complete=wait,
|
||||||
timeout=timeout,
|
timeout=timeout,
|
||||||
)
|
)
|
||||||
if command == "rotate_by":
|
if command == "rotate_by":
|
||||||
return gripper.rotate_by(
|
return gripper.rotate_by(
|
||||||
float(msg.position),
|
float(msg.position),
|
||||||
speed=int(msg.speed) if msg.speed else None,
|
speed=int(msg.speed) if msg.speed else None,
|
||||||
torque=int(msg.force) if msg.force else None,
|
torque=int(msg.force) if msg.force else None,
|
||||||
wait_complete=wait,
|
wait_complete=wait,
|
||||||
timeout=timeout,
|
timeout=timeout,
|
||||||
)
|
)
|
||||||
if command == "rotate_revolutions":
|
if command == "rotate_revolutions":
|
||||||
return gripper.rotate_revolutions(
|
return gripper.rotate_revolutions(
|
||||||
float(msg.position),
|
float(msg.position),
|
||||||
speed=int(msg.speed) if msg.speed else None,
|
speed=int(msg.speed) if msg.speed else None,
|
||||||
torque=int(msg.force) if msg.force else None,
|
torque=int(msg.force) if msg.force else None,
|
||||||
wait_complete=wait,
|
wait_complete=wait,
|
||||||
timeout=timeout,
|
timeout=timeout,
|
||||||
)
|
)
|
||||||
if command == "home_rotation":
|
if command == "home_rotation":
|
||||||
return gripper.home_rotation(
|
return gripper.home_rotation(
|
||||||
speed=int(msg.speed) if msg.speed else None,
|
speed=int(msg.speed) if msg.speed else None,
|
||||||
torque=int(msg.force) if msg.force else None,
|
torque=int(msg.force) if msg.force else None,
|
||||||
wait_complete=wait,
|
wait_complete=wait,
|
||||||
timeout=timeout,
|
timeout=timeout,
|
||||||
)
|
)
|
||||||
if command == "set_force":
|
if command == "set_force":
|
||||||
gripper.set_rotation_torque(int(msg.force))
|
gripper.set_rotation_torque(int(msg.force))
|
||||||
return gripper.get_status()
|
return gripper.get_status()
|
||||||
if command == "set_speed":
|
if command == "set_speed":
|
||||||
gripper.set_rotation_speed(int(msg.speed))
|
gripper.set_rotation_speed(int(msg.speed))
|
||||||
return gripper.get_status()
|
return gripper.get_status()
|
||||||
if command == "set_finger_force":
|
if command == "set_finger_force":
|
||||||
gripper.set_finger_force(int(msg.force))
|
gripper.set_finger_force(int(msg.force))
|
||||||
return gripper.get_status()
|
return gripper.get_status()
|
||||||
if command == "set_finger_speed":
|
if command == "set_finger_speed":
|
||||||
gripper.set_finger_speed(int(msg.speed))
|
gripper.set_finger_speed(int(msg.speed))
|
||||||
return gripper.get_status()
|
return gripper.get_status()
|
||||||
|
|
||||||
status = gripper.get_status()
|
status = gripper.get_status()
|
||||||
status.state = GripperState.ERROR
|
status.state = GripperState.ERROR
|
||||||
status.error_msg = f"不支持的命令 {command!r}（side={side}）"
|
status.error_msg = f"不支持的命令 {command!r}（side={side}）"
|
||||||
return status
|
return status
|
||||||
|
|
||||||
def _on_command(self, side: str, msg: GripperCommandMsg) -> None:
|
def _on_command(self, side: str, msg: GripperCommandMsg) -> None:
|
||||||
with self._locks[side]:
|
with self._locks[side]:
|
||||||
status = self._execute(side, msg)
|
status = self._execute(side, msg)
|
||||||
if status.state == GripperState.ERROR:
|
if status.state == GripperState.ERROR:
|
||||||
self.get_logger().warn(f"{side} 命令 {msg.command!r} 失败：{status.error_msg}")
|
self.get_logger().warn(f"{side} 命令 {msg.command!r} 失败：{status.error_msg}")
|
||||||
|
|
||||||
def _on_execute(self, side: str, request: GripperExecute.Request, response: GripperExecute.Response):
|
def _on_execute(self, side: str, request: GripperExecute.Request, response: GripperExecute.Response):
|
||||||
with self._locks[side]:
|
with self._locks[side]:
|
||||||
status = self._execute(side, request.command)
|
status = self._execute(side, request.command)
|
||||||
response.success = status.state != GripperState.ERROR and status.connected
|
response.success = status.state != GripperState.ERROR and status.connected
|
||||||
response.message = status.error_msg or "OK"
|
response.message = status.error_msg or "OK"
|
||||||
response.status = self._to_status_msg(side, status)
|
response.status = self._to_status_msg(side, status)
|
||||||
return response
|
return response
|
||||||
|
|
||||||
def _to_status_msg(self, side: str, status) -> GripperStatusMsg:
|
def _to_status_msg(self, side: str, status) -> GripperStatusMsg:
|
||||||
gripper = self._gripper(side)
|
gripper = self._gripper(side)
|
||||||
msg = GripperStatusMsg()
|
msg = GripperStatusMsg()
|
||||||
msg.header.stamp = self.get_clock().now().to_msg()
|
msg.header.stamp = self.get_clock().now().to_msg()
|
||||||
msg.header.frame_id = f"{side}_gripper"
|
msg.header.frame_id = f"{side}_gripper"
|
||||||
msg.name = gripper.name
|
msg.name = gripper.name
|
||||||
msg.gripper_type = gripper.gripper_type.name.lower()
|
msg.gripper_type = gripper.gripper_type.name.lower()
|
||||||
msg.state = status.state.name
|
msg.state = status.state.name
|
||||||
msg.connected = bool(status.connected)
|
msg.connected = bool(status.connected)
|
||||||
msg.initialized = bool(status.initialized)
|
msg.initialized = bool(status.initialized)
|
||||||
msg.is_moving = bool(status.is_moving)
|
msg.is_moving = bool(status.is_moving)
|
||||||
msg.is_gripped = bool(status.is_gripped)
|
msg.is_gripped = bool(status.is_gripped)
|
||||||
msg.position = float(status.position)
|
msg.position = float(status.position)
|
||||||
msg.finger_position_per_mille = float(status.finger_position_per_mille)
|
msg.finger_position_per_mille = float(status.finger_position_per_mille)
|
||||||
msg.rotation_deg = float(status.rotation_deg)
|
msg.rotation_deg = float(status.rotation_deg)
|
||||||
msg.gripper_state = int(status.gripper_state)
|
msg.gripper_state = int(status.gripper_state)
|
||||||
msg.rotation_state = int(status.rotation_state)
|
msg.rotation_state = int(status.rotation_state)
|
||||||
msg.velocity = float(status.velocity)
|
msg.velocity = float(status.velocity)
|
||||||
msg.force = float(status.force)
|
msg.force = float(status.force)
|
||||||
msg.error_msg = status.error_msg or ""
|
msg.error_msg = status.error_msg or ""
|
||||||
return msg
|
return msg
|
||||||
|
|
||||||
def _publish_status(self) -> None:
|
def _publish_status(self) -> None:
|
||||||
if "left" in self._sides:
|
if "left" in self._sides:
|
||||||
left_status = self._manager.left.get_status()
|
left_status = self._manager.left.get_status()
|
||||||
if "left" in self._connect_errors and not left_status.connected:
|
if "left" in self._connect_errors and not left_status.connected:
|
||||||
left_status.error_msg = self._connect_errors["left"]
|
left_status.error_msg = self._connect_errors["left"]
|
||||||
self._left_pub.publish(self._to_status_msg("left", left_status))
|
self._left_pub.publish(self._to_status_msg("left", left_status))
|
||||||
if "right" in self._sides:
|
if "right" in self._sides:
|
||||||
right_status = self._manager.right.get_status()
|
right_status = self._manager.right.get_status()
|
||||||
if "right" in self._connect_errors and not right_status.connected:
|
if "right" in self._connect_errors and not right_status.connected:
|
||||||
right_status.error_msg = self._connect_errors["right"]
|
right_status.error_msg = self._connect_errors["right"]
|
||||||
self._right_pub.publish(self._to_status_msg("right", right_status))
|
self._right_pub.publish(self._to_status_msg("right", right_status))
|
||||||
|
|
||||||
def destroy_node(self) -> bool:
|
def destroy_node(self) -> bool:
|
||||||
try:
|
try:
|
||||||
self._manager.disconnect_all()
|
self._manager.disconnect_all()
|
||||||
finally:
|
finally:
|
||||||
return super().destroy_node()
|
return super().destroy_node()
|
||||||
|
|
||||||
|
|
||||||
def main() -> None:
|
def main() -> None:
|
||||||
rclpy.init()
|
rclpy.init()
|
||||||
node = DualGripperNode()
|
node = DualGripperNode()
|
||||||
executor = MultiThreadedExecutor(num_threads=4)
|
executor = MultiThreadedExecutor(num_threads=4)
|
||||||
try:
|
try:
|
||||||
rclpy.spin(node, executor=executor)
|
rclpy.spin(node, executor=executor)
|
||||||
finally:
|
finally:
|
||||||
node.destroy_node()
|
node.destroy_node()
|
||||||
rclpy.shutdown()
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
main()
|
main()
|
||||||
|
|||||||
@@ -1,188 +1,188 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""在当前机械臂的 D405 彩色话题上做入孔检测。只检测，不做插入动作。"""
|
"""在当前机械臂的 D405 彩色话题上做入孔检测。只检测，不做插入动作。"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import os
|
import os
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import rclpy
|
import rclpy
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
from rclpy.qos import qos_profile_sensor_data
|
from rclpy.qos import qos_profile_sensor_data
|
||||||
from sensor_msgs.msg import Image
|
from sensor_msgs.msg import Image
|
||||||
from std_msgs.msg import Float32MultiArray
|
from std_msgs.msg import Float32MultiArray
|
||||||
|
|
||||||
from arm_application.handeye_realsense import resolve_realsense_topics
|
from arm_application.handeye_realsense import resolve_realsense_topics
|
||||||
from arm_application.hole_detect_hough import HoleStabilizer
|
from arm_application.hole_detect_hough import HoleStabilizer
|
||||||
from arm_application.hole_detector import (
|
from arm_application.hole_detector import (
|
||||||
HoleDetection,
|
HoleDetection,
|
||||||
HoleDetectResult,
|
HoleDetectResult,
|
||||||
HoleDetector,
|
HoleDetector,
|
||||||
config_from_site,
|
config_from_site,
|
||||||
weights_from_site,
|
weights_from_site,
|
||||||
)
|
)
|
||||||
from arm_application.site_io import load_site_dict
|
from arm_application.site_io import load_site_dict
|
||||||
from arm_msgs.msg import HoleDetection as HoleDetectionMsg
|
from arm_msgs.msg import HoleDetection as HoleDetectionMsg
|
||||||
from arm_msgs.msg import HoleDetections
|
from arm_msgs.msg import HoleDetections
|
||||||
|
|
||||||
|
|
||||||
def _as_bool(val, default: bool = False) -> bool:
|
def _as_bool(val, default: bool = False) -> bool:
|
||||||
if val is None:
|
if val is None:
|
||||||
return default
|
return default
|
||||||
if isinstance(val, str):
|
if isinstance(val, str):
|
||||||
return val.strip().lower() in ("1", "true", "yes", "on")
|
return val.strip().lower() in ("1", "true", "yes", "on")
|
||||||
return bool(val)
|
return bool(val)
|
||||||
|
|
||||||
|
|
||||||
def _bgr_copy(msg: Image) -> np.ndarray:
|
def _bgr_copy(msg: Image) -> np.ndarray:
|
||||||
h, w = int(msg.height), int(msg.width)
|
h, w = int(msg.height), int(msg.width)
|
||||||
enc = (msg.encoding or "").lower()
|
enc = (msg.encoding or "").lower()
|
||||||
buf = np.frombuffer(msg.data, dtype=np.uint8)
|
buf = np.frombuffer(msg.data, dtype=np.uint8)
|
||||||
if enc == "bgr8":
|
if enc == "bgr8":
|
||||||
return buf.reshape(h, w, 3).copy()
|
return buf.reshape(h, w, 3).copy()
|
||||||
if enc == "rgb8":
|
if enc == "rgb8":
|
||||||
return buf.reshape(h, w, 3)[:, :, ::-1].copy()
|
return buf.reshape(h, w, 3)[:, :, ::-1].copy()
|
||||||
raise RuntimeError(f"Unsupported encoding {msg.encoding!r}")
|
raise RuntimeError(f"Unsupported encoding {msg.encoding!r}")
|
||||||
|
|
||||||
|
|
||||||
def _bgr_to_msg(image: np.ndarray, stamp, frame_id: str) -> Image:
|
def _bgr_to_msg(image: np.ndarray, stamp, frame_id: str) -> Image:
|
||||||
msg = Image()
|
msg = Image()
|
||||||
msg.header.stamp = stamp
|
msg.header.stamp = stamp
|
||||||
msg.header.frame_id = frame_id
|
msg.header.frame_id = frame_id
|
||||||
msg.height = int(image.shape[0])
|
msg.height = int(image.shape[0])
|
||||||
msg.width = int(image.shape[1])
|
msg.width = int(image.shape[1])
|
||||||
msg.encoding = "bgr8"
|
msg.encoding = "bgr8"
|
||||||
msg.step = int(image.shape[1] * 3)
|
msg.step = int(image.shape[1] * 3)
|
||||||
msg.data = image.tobytes()
|
msg.data = image.tobytes()
|
||||||
return msg
|
return msg
|
||||||
|
|
||||||
|
|
||||||
class HoleDetectNode(Node):
|
class HoleDetectNode(Node):
|
||||||
def __init__(self) -> None:
|
def __init__(self) -> None:
|
||||||
super().__init__("hole_detect")
|
super().__init__("hole_detect")
|
||||||
self.declare_parameter("camera_source", os.environ.get("SITE_CAM_NAME") or "d405")
|
self.declare_parameter("camera_source", os.environ.get("SITE_CAM_NAME") or "d405")
|
||||||
self.declare_parameter("image_topic", "")
|
self.declare_parameter("image_topic", "")
|
||||||
self.declare_parameter("weights", "")
|
self.declare_parameter("weights", "")
|
||||||
self.declare_parameter("config_file", "")
|
self.declare_parameter("config_file", "")
|
||||||
self.declare_parameter("backend", "two_stage")
|
self.declare_parameter("backend", "two_stage")
|
||||||
self.declare_parameter("device", os.environ.get("SITE_YOLO_DEVICE") or "0")
|
self.declare_parameter("device", os.environ.get("SITE_YOLO_DEVICE") or "0")
|
||||||
self.declare_parameter("preview", False)
|
self.declare_parameter("preview", False)
|
||||||
|
|
||||||
site = load_site_dict()
|
site = load_site_dict()
|
||||||
cam = str(self.get_parameter("camera_source").value or "d405")
|
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 ""))
|
image_topic, _info = resolve_realsense_topics(cam, image_topic=str(self.get_parameter("image_topic").value or ""))
|
||||||
cfg = config_from_site(site)
|
cfg = config_from_site(site)
|
||||||
param_backend = str(self.get_parameter("backend").value or "").strip().lower()
|
param_backend = str(self.get_parameter("backend").value or "").strip().lower()
|
||||||
if param_backend:
|
if param_backend:
|
||||||
cfg["backend"] = param_backend
|
cfg["backend"] = param_backend
|
||||||
backend = str(cfg.get("backend") or "two_stage")
|
backend = str(cfg.get("backend") or "two_stage")
|
||||||
weights = str(self.get_parameter("weights").value or "").strip() or weights_from_site(site)
|
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._preview = _as_bool(self.get_parameter("preview").value, False)
|
||||||
self._detector = HoleDetector(
|
self._detector = HoleDetector(
|
||||||
model_path=weights or None,
|
model_path=weights or None,
|
||||||
config=cfg,
|
config=cfg,
|
||||||
device=str(self.get_parameter("device").value or "0"),
|
device=str(self.get_parameter("device").value or "0"),
|
||||||
)
|
)
|
||||||
self._stabilizer = (
|
self._stabilizer = (
|
||||||
HoleStabilizer(cfg) if backend in ("two_stage", "hough_roi") else None
|
HoleStabilizer(cfg) if backend in ("two_stage", "hough_roi") else None
|
||||||
)
|
)
|
||||||
self._pub = self.create_publisher(HoleDetections, "/hole_detect/result", 10)
|
self._pub = self.create_publisher(HoleDetections, "/hole_detect/result", 10)
|
||||||
self._arr_pub = self.create_publisher(Float32MultiArray, "/hole_detect/holes", 10)
|
self._arr_pub = self.create_publisher(Float32MultiArray, "/hole_detect/holes", 10)
|
||||||
self._overlay_pub = self.create_publisher(Image, "/hole_detect/overlay", 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.create_subscription(Image, image_topic, self._on_image, qos_profile_sensor_data)
|
||||||
self.get_logger().info(
|
self.get_logger().info(
|
||||||
f"hole_detect camera={cam} topic={image_topic} backend={backend} "
|
f"hole_detect camera={cam} topic={image_topic} backend={backend} "
|
||||||
f"weights={self._detector.model_path} site overlay=hole_detect"
|
f"weights={self._detector.model_path} site overlay=hole_detect"
|
||||||
)
|
)
|
||||||
|
|
||||||
def _on_image(self, msg: Image) -> None:
|
def _on_image(self, msg: Image) -> None:
|
||||||
try:
|
try:
|
||||||
bgr = _bgr_copy(msg)
|
bgr = _bgr_copy(msg)
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
self.get_logger().error(str(exc))
|
self.get_logger().error(str(exc))
|
||||||
return
|
return
|
||||||
result = self._detector.detect(bgr)
|
result = self._detector.detect(bgr)
|
||||||
if self._stabilizer is not None:
|
if self._stabilizer is not None:
|
||||||
result = self._stabilize(bgr, result)
|
result = self._stabilize(bgr, result)
|
||||||
out = HoleDetections()
|
out = HoleDetections()
|
||||||
out.header = msg.header
|
out.header = msg.header
|
||||||
out.ok = bool(result.status)
|
out.ok = bool(result.status)
|
||||||
out.message = result.msg
|
out.message = result.msg
|
||||||
for hole in result.holes:
|
for hole in result.holes:
|
||||||
hmsg = HoleDetectionMsg()
|
hmsg = HoleDetectionMsg()
|
||||||
hmsg.u = float(hole.center_px[0])
|
hmsg.u = float(hole.center_px[0])
|
||||||
hmsg.v = float(hole.center_px[1])
|
hmsg.v = float(hole.center_px[1])
|
||||||
hmsg.radius_px = float(hole.radius_px)
|
hmsg.radius_px = float(hole.radius_px)
|
||||||
hmsg.confidence = float(hole.confidence)
|
hmsg.confidence = float(hole.confidence)
|
||||||
hmsg.label = hole.label or ""
|
hmsg.label = hole.label or ""
|
||||||
if hole.bbox:
|
if hole.bbox:
|
||||||
hmsg.bbox = [int(v) for v in hole.bbox]
|
hmsg.bbox = [int(v) for v in hole.bbox]
|
||||||
out.holes.append(hmsg)
|
out.holes.append(hmsg)
|
||||||
self._pub.publish(out)
|
self._pub.publish(out)
|
||||||
|
|
||||||
arr = Float32MultiArray()
|
arr = Float32MultiArray()
|
||||||
data = [1.0 if result.status else 0.0, float(len(result.holes))]
|
data = [1.0 if result.status else 0.0, float(len(result.holes))]
|
||||||
for hole in result.holes:
|
for hole in result.holes:
|
||||||
data.extend([hole.center_px[0], hole.center_px[1], hole.radius_px, hole.confidence])
|
data.extend([hole.center_px[0], hole.center_px[1], hole.radius_px, hole.confidence])
|
||||||
arr.data = data
|
arr.data = data
|
||||||
self._arr_pub.publish(arr)
|
self._arr_pub.publish(arr)
|
||||||
|
|
||||||
overlay = result.draw_img if result.draw_img is not None else bgr
|
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))
|
self._overlay_pub.publish(_bgr_to_msg(overlay, msg.header.stamp, msg.header.frame_id))
|
||||||
if self._preview:
|
if self._preview:
|
||||||
import cv2
|
import cv2
|
||||||
|
|
||||||
cv2.imshow("hole_detect", overlay)
|
cv2.imshow("hole_detect", overlay)
|
||||||
cv2.waitKey(1)
|
cv2.waitKey(1)
|
||||||
|
|
||||||
def _stabilize(self, bgr, result: HoleDetectResult) -> HoleDetectResult:
|
def _stabilize(self, bgr, result: HoleDetectResult) -> HoleDetectResult:
|
||||||
raw = result.raw if isinstance(result.raw, dict) else {}
|
raw = result.raw if isinstance(result.raw, dict) else {}
|
||||||
payload = {
|
payload = {
|
||||||
"status": result.status,
|
"status": result.status,
|
||||||
"msg": result.msg,
|
"msg": result.msg,
|
||||||
"hole_points": raw.get("hole_points")
|
"hole_points": raw.get("hole_points")
|
||||||
or [list(h.center_px) for h in result.holes],
|
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],
|
"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),
|
"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,
|
"draw_img": result.draw_img,
|
||||||
"gate_block": bool(raw.get("gate_block")),
|
"gate_block": bool(raw.get("gate_block")),
|
||||||
}
|
}
|
||||||
stab = self._stabilizer.update(bgr, payload)
|
stab = self._stabilizer.update(bgr, payload)
|
||||||
holes: list[HoleDetection] = []
|
holes: list[HoleDetection] = []
|
||||||
radii = stab.get("hole_radii") or []
|
radii = stab.get("hole_radii") or []
|
||||||
bbox = stab.get("bbox")
|
bbox = stab.get("bbox")
|
||||||
bbox_t = tuple(int(v) for v in bbox) if bbox and len(bbox) == 4 else None
|
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 []):
|
for i, pt in enumerate(stab.get("hole_points") or []):
|
||||||
rr = float(radii[i]) if i < len(radii) else 0.0
|
rr = float(radii[i]) if i < len(radii) else 0.0
|
||||||
holes.append(
|
holes.append(
|
||||||
HoleDetection(
|
HoleDetection(
|
||||||
center_px=(float(pt[0]), float(pt[1])),
|
center_px=(float(pt[0]), float(pt[1])),
|
||||||
radius_px=rr,
|
radius_px=rr,
|
||||||
confidence=1.0,
|
confidence=1.0,
|
||||||
bbox=bbox_t,
|
bbox=bbox_t,
|
||||||
label="left" if i == 0 else "right" if i == 1 else f"h{i + 1}",
|
label="left" if i == 0 else "right" if i == 1 else f"h{i + 1}",
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
return HoleDetectResult(
|
return HoleDetectResult(
|
||||||
bool(stab.get("status")),
|
bool(stab.get("status")),
|
||||||
str(stab.get("msg") or ""),
|
str(stab.get("msg") or ""),
|
||||||
holes,
|
holes,
|
||||||
stab.get("draw_img", result.draw_img),
|
stab.get("draw_img", result.draw_img),
|
||||||
raw=stab,
|
raw=stab,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
def main() -> None:
|
def main() -> None:
|
||||||
rclpy.init()
|
rclpy.init()
|
||||||
node = HoleDetectNode()
|
node = HoleDetectNode()
|
||||||
try:
|
try:
|
||||||
rclpy.spin(node)
|
rclpy.spin(node)
|
||||||
except KeyboardInterrupt:
|
except KeyboardInterrupt:
|
||||||
pass
|
pass
|
||||||
finally:
|
finally:
|
||||||
node.destroy_node()
|
node.destroy_node()
|
||||||
rclpy.shutdown()
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
main()
|
main()
|
||||||
|
|||||||
@@ -1,367 +1,367 @@
|
|||||||
"""孔定位模块：加载已训练模型，输出孔中心像素坐标与置信度。
|
"""孔定位模块：加载已训练模型，输出孔中心像素坐标与置信度。
|
||||||
|
|
||||||
与工件定位（thread_gauge）完全分离，可独立替换模型与参数。
|
与工件定位（thread_gauge）完全分离，可独立替换模型与参数。
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from typing import Any
|
from typing import Any
|
||||||
|
|
||||||
import cv2
|
import cv2
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
_PKG_SHARE_FALLBACK = Path(__file__).resolve().parents[1] / "config"
|
_PKG_SHARE_FALLBACK = Path(__file__).resolve().parents[1] / "config"
|
||||||
DEFAULT_CONFIG = _PKG_SHARE_FALLBACK / "hole_insert.yaml"
|
DEFAULT_CONFIG = _PKG_SHARE_FALLBACK / "hole_insert.yaml"
|
||||||
|
|
||||||
# 仓库里**实际存在**的孔/工件检测模型，相对仓库根。
|
# 仓库里**实际存在**的孔/工件检测模型，相对仓库根。
|
||||||
# 以前这个位置上写的是 ``models/hole_insert/best.pt`` —— 文件根本不存在，属于悬空引用：
|
# 以前这个位置上写的是 ``models/hole_insert/best.pt`` —— 文件根本不存在，属于悬空引用：
|
||||||
# 配置齐全时永远走不到这条分支，一旦 site.yaml 缺键就报"模型文件不存在"，
|
# 配置齐全时永远走不到这条分支，一旦 site.yaml 缺键就报"模型文件不存在"，
|
||||||
# 而打印出来的路径看着特别合理，很容易被误判成"文件被人误删了"。
|
# 而打印出来的路径看着特别合理，很容易被误判成"文件被人误删了"。
|
||||||
# 改动这个常量时，顺手跑 ``bash scripts/weights.sh doctor`` 确认没有悬空引用。
|
# 改动这个常量时，顺手跑 ``bash scripts/weights.sh doctor`` 确认没有悬空引用。
|
||||||
_CANONICAL_HOLE_MODEL = "models/hole_insert/detect_best_1.onnx"
|
_CANONICAL_HOLE_MODEL = "models/hole_insert/detect_best_1.onnx"
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class HoleDetection:
|
class HoleDetection:
|
||||||
"""单孔检测结果。"""
|
"""单孔检测结果。"""
|
||||||
|
|
||||||
center_px: tuple[float, float]
|
center_px: tuple[float, float]
|
||||||
radius_px: float
|
radius_px: float
|
||||||
confidence: float
|
confidence: float
|
||||||
bbox: tuple[int, int, int, int] | None = None
|
bbox: tuple[int, int, int, int] | None = None
|
||||||
cls_id: int = 0
|
cls_id: int = 0
|
||||||
label: str = "" # left / right
|
label: str = "" # left / right
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class HoleDetectResult:
|
class HoleDetectResult:
|
||||||
"""孔定位整体结果。"""
|
"""孔定位整体结果。"""
|
||||||
|
|
||||||
status: bool
|
status: bool
|
||||||
msg: str
|
msg: str
|
||||||
holes: list[HoleDetection]
|
holes: list[HoleDetection]
|
||||||
draw_img: np.ndarray
|
draw_img: np.ndarray
|
||||||
raw: dict[str, Any] | None = None
|
raw: dict[str, Any] | None = None
|
||||||
|
|
||||||
|
|
||||||
def load_hole_config(path: str | Path | None = None) -> dict[str, Any]:
|
def load_hole_config(path: str | Path | None = None) -> dict[str, Any]:
|
||||||
cfg_path = Path(path) if path else DEFAULT_CONFIG
|
cfg_path = Path(path) if path else DEFAULT_CONFIG
|
||||||
with open(cfg_path, encoding="utf-8") as f:
|
with open(cfg_path, encoding="utf-8") as f:
|
||||||
data = yaml.safe_load(f) or {}
|
data = yaml.safe_load(f) or {}
|
||||||
if not isinstance(data, dict):
|
if not isinstance(data, dict):
|
||||||
raise ValueError(f"孔定位配置格式错误: {cfg_path}")
|
raise ValueError(f"孔定位配置格式错误: {cfg_path}")
|
||||||
return data
|
return data
|
||||||
|
|
||||||
|
|
||||||
_HOLE_SITE_SKIP = {"weights", "workpiece_weights", "config"}
|
_HOLE_SITE_SKIP = {"weights", "workpiece_weights", "config"}
|
||||||
|
|
||||||
|
|
||||||
def config_from_site(data: dict[str, Any] | None = None) -> dict[str, Any]:
|
def config_from_site(data: dict[str, Any] | None = None) -> dict[str, Any]:
|
||||||
"""先取包内 yaml 默认值，再用 ``site.yaml`` 的 ``hole_detect:`` 覆盖。
|
"""先取包内 yaml 默认值，再用 ``site.yaml`` 的 ``hole_detect:`` 覆盖。
|
||||||
|
|
||||||
以 ``site.yaml`` 为准（operator 的唯一数据源）；``data`` 参数用于测试时直接喂字典。
|
以 ``site.yaml`` 为准（operator 的唯一数据源）；``data`` 参数用于测试时直接喂字典。
|
||||||
"""
|
"""
|
||||||
from arm_application.site_io import deep_merge, load_site_dict, ros2_root
|
from arm_application.site_io import deep_merge, load_site_dict, ros2_root
|
||||||
|
|
||||||
site = data if data is not None else load_site_dict()
|
site = data if data is not None else load_site_dict()
|
||||||
hd = dict(site.get("hole_detect") or {})
|
hd = dict(site.get("hole_detect") or {})
|
||||||
backend = str(hd.get("backend") or "two_stage").strip().lower()
|
backend = str(hd.get("backend") or "two_stage").strip().lower()
|
||||||
cfg_path = str(hd.get("config") or "").strip()
|
cfg_path = str(hd.get("config") or "").strip()
|
||||||
if not cfg_path:
|
if not cfg_path:
|
||||||
root = ros2_root()
|
root = ros2_root()
|
||||||
if backend in ("two_stage", "hough_roi"):
|
if backend in ("two_stage", "hough_roi"):
|
||||||
cfg_path = str(root / "src" / "arm_application" / "config" / "hole_detect.yaml")
|
cfg_path = str(root / "src" / "arm_application" / "config" / "hole_detect.yaml")
|
||||||
else:
|
else:
|
||||||
cfg_path = str(root / "src" / "arm_application" / "config" / "hole_insert.yaml")
|
cfg_path = str(root / "src" / "arm_application" / "config" / "hole_insert.yaml")
|
||||||
base = load_hole_config(cfg_path) if Path(cfg_path).is_file() else {}
|
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 != ""}
|
overlay = {k: v for k, v in hd.items() if k not in _HOLE_SITE_SKIP and v != ""}
|
||||||
merged = deep_merge(base, overlay)
|
merged = deep_merge(base, overlay)
|
||||||
merged["backend"] = backend
|
merged["backend"] = backend
|
||||||
return merged
|
return merged
|
||||||
|
|
||||||
|
|
||||||
def weights_from_site(data: dict[str, Any] | None = None) -> str:
|
def weights_from_site(data: dict[str, Any] | None = None) -> str:
|
||||||
from arm_application.site_io import load_site_dict, ros2_root
|
from arm_application.site_io import load_site_dict, ros2_root
|
||||||
|
|
||||||
site = data if data is not None else load_site_dict()
|
site = data if data is not None else load_site_dict()
|
||||||
hd = site.get("hole_detect") or {}
|
hd = site.get("hole_detect") or {}
|
||||||
paths = site.get("paths") or {}
|
paths = site.get("paths") or {}
|
||||||
backend = str(hd.get("backend") or "two_stage").strip().lower()
|
backend = str(hd.get("backend") or "two_stage").strip().lower()
|
||||||
if backend in ("two_stage", "hough_roi"):
|
if backend in ("two_stage", "hough_roi"):
|
||||||
return str(
|
return str(
|
||||||
hd.get("workpiece_weights")
|
hd.get("workpiece_weights")
|
||||||
or paths.get("hole_workpiece_weights")
|
or paths.get("hole_workpiece_weights")
|
||||||
or paths.get("yolo_weights")
|
or paths.get("yolo_weights")
|
||||||
or ""
|
or ""
|
||||||
)
|
)
|
||||||
return str(
|
return str(
|
||||||
hd.get("weights")
|
hd.get("weights")
|
||||||
or paths.get("hole_weights")
|
or paths.get("hole_weights")
|
||||||
or str(ros2_root() / _CANONICAL_HOLE_MODEL)
|
or str(ros2_root() / _CANONICAL_HOLE_MODEL)
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
def _default_model_path() -> Path:
|
def _default_model_path() -> Path:
|
||||||
"""未显式给 ``model_path`` 时的兜底模型路径。
|
"""未显式给 ``model_path`` 时的兜底模型路径。
|
||||||
|
|
||||||
以前这里枚举了 ``hole_insert/best.pt`` / ``hole_best.pt`` / ``yolo/detect_best.pt``，
|
以前这里枚举了 ``hole_insert/best.pt`` / ``hole_best.pt`` / ``yolo/detect_best.pt``，
|
||||||
三个文件在本仓库里**都不存在**，函数却会 ``return candidates[0]`` ——
|
三个文件在本仓库里**都不存在**，函数却会 ``return candidates[0]`` ——
|
||||||
于是永远返回一个不存在的路径（同 ``weights_from_site`` 里那个悬空的 best.pt）。
|
于是永远返回一个不存在的路径（同 ``weights_from_site`` 里那个悬空的 best.pt）。
|
||||||
|
|
||||||
现在以 site.yaml 为准（它才是真正的来源），它给的路径存在就用它；
|
现在以 site.yaml 为准（它才是真正的来源），它给的路径存在就用它；
|
||||||
否则退到仓库里实际存在的规范模型。这样报错信息里的路径至少是对的。
|
否则退到仓库里实际存在的规范模型。这样报错信息里的路径至少是对的。
|
||||||
"""
|
"""
|
||||||
configured = weights_from_site()
|
configured = weights_from_site()
|
||||||
if configured:
|
if configured:
|
||||||
p = Path(configured).expanduser()
|
p = Path(configured).expanduser()
|
||||||
if p.is_file():
|
if p.is_file():
|
||||||
return p
|
return p
|
||||||
|
|
||||||
from arm_application.site_io import ros2_root
|
from arm_application.site_io import ros2_root
|
||||||
|
|
||||||
return ros2_root() / _CANONICAL_HOLE_MODEL
|
return ros2_root() / _CANONICAL_HOLE_MODEL
|
||||||
|
|
||||||
|
|
||||||
class HoleDetector:
|
class HoleDetector:
|
||||||
"""孔定位检测器：支持 YOLO / HoughCircles 双模式，可配置切换。"""
|
"""孔定位检测器：支持 YOLO / HoughCircles 双模式，可配置切换。"""
|
||||||
|
|
||||||
def __init__(
|
def __init__(
|
||||||
self,
|
self,
|
||||||
model_path: str | Path | None = None,
|
model_path: str | Path | None = None,
|
||||||
config: dict[str, Any] | None = None,
|
config: dict[str, Any] | None = None,
|
||||||
device: str | int = 0,
|
device: str | int = 0,
|
||||||
) -> None:
|
) -> None:
|
||||||
self.cfg = config or load_hole_config()
|
self.cfg = config or load_hole_config()
|
||||||
self.device = device
|
self.device = device
|
||||||
self.model_path = Path(model_path) if model_path else _default_model_path()
|
self.model_path = Path(model_path) if model_path else _default_model_path()
|
||||||
self._detector = None
|
self._detector = None
|
||||||
self._backend = self.cfg.get("backend", "yolo").lower()
|
self._backend = self.cfg.get("backend", "yolo").lower()
|
||||||
|
|
||||||
def _load_yolo(self):
|
def _load_yolo(self):
|
||||||
if self._detector is not None:
|
if self._detector is not None:
|
||||||
return self._detector
|
return self._detector
|
||||||
from arm_application.yolo_detector import YoloDetector, resolve_weights
|
from arm_application.yolo_detector import YoloDetector, resolve_weights
|
||||||
|
|
||||||
weights = resolve_weights(
|
weights = resolve_weights(
|
||||||
self.model_path,
|
self.model_path,
|
||||||
backend=str(self.cfg.get("yolo_backend") or "auto"),
|
backend=str(self.cfg.get("yolo_backend") or "auto"),
|
||||||
)
|
)
|
||||||
self._detector = YoloDetector(
|
self._detector = YoloDetector(
|
||||||
weights,
|
weights,
|
||||||
device=str(self.device),
|
device=str(self.device),
|
||||||
conf=float(self.cfg.get("conf", 0.25)),
|
conf=float(self.cfg.get("conf", 0.25)),
|
||||||
iou=float(self.cfg.get("iou", 0.7)),
|
iou=float(self.cfg.get("iou", 0.7)),
|
||||||
imgsz=int(self.cfg.get("imgsz", 640)),
|
imgsz=int(self.cfg.get("imgsz", 640)),
|
||||||
)
|
)
|
||||||
return self._detector
|
return self._detector
|
||||||
|
|
||||||
def detect(self, image_bgr: np.ndarray) -> HoleDetectResult:
|
def detect(self, image_bgr: np.ndarray) -> HoleDetectResult:
|
||||||
"""检测图像中的孔，返回像素坐标列表。"""
|
"""检测图像中的孔，返回像素坐标列表。"""
|
||||||
if image_bgr is None or image_bgr.size == 0:
|
if image_bgr is None or image_bgr.size == 0:
|
||||||
return HoleDetectResult(False, "输入图像无效", [], np.zeros((480, 640, 3), dtype=np.uint8))
|
return HoleDetectResult(False, "输入图像无效", [], np.zeros((480, 640, 3), dtype=np.uint8))
|
||||||
|
|
||||||
draw_img = image_bgr.copy()
|
draw_img = image_bgr.copy()
|
||||||
backend = self._backend
|
backend = self._backend
|
||||||
|
|
||||||
if backend in {"yolo", "faster_rcnn"}:
|
if backend in {"yolo", "faster_rcnn"}:
|
||||||
return self._detect_by_model(image_bgr, draw_img)
|
return self._detect_by_model(image_bgr, draw_img)
|
||||||
if backend in {"two_stage", "hough_roi"}:
|
if backend in {"two_stage", "hough_roi"}:
|
||||||
return self._detect_two_stage(image_bgr, draw_img)
|
return self._detect_two_stage(image_bgr, draw_img)
|
||||||
if backend == "hough":
|
if backend == "hough":
|
||||||
return self._detect_by_hough(image_bgr, draw_img)
|
return self._detect_by_hough(image_bgr, draw_img)
|
||||||
return HoleDetectResult(False, f"未知 backend: {backend}", [], draw_img)
|
return HoleDetectResult(False, f"未知 backend: {backend}", [], draw_img)
|
||||||
|
|
||||||
def _detect_two_stage(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
|
def _detect_two_stage(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
|
||||||
from arm_application.hole_detect_hough import detect_holes
|
from arm_application.hole_detect_hough import detect_holes
|
||||||
|
|
||||||
try:
|
try:
|
||||||
detector = self._load_yolo()
|
detector = self._load_yolo()
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
return HoleDetectResult(False, f"模型加载失败: {exc}", [], draw_img)
|
return HoleDetectResult(False, f"模型加载失败: {exc}", [], draw_img)
|
||||||
|
|
||||||
raw = detect_holes(image, detector, config=self.cfg, device=self.device)
|
raw = detect_holes(image, detector, config=self.cfg, device=self.device)
|
||||||
holes: list[HoleDetection] = []
|
holes: list[HoleDetection] = []
|
||||||
points = raw.get("hole_points") or []
|
points = raw.get("hole_points") or []
|
||||||
radii = raw.get("hole_radii") or []
|
radii = raw.get("hole_radii") or []
|
||||||
bbox = raw.get("bbox")
|
bbox = raw.get("bbox")
|
||||||
bbox_t = tuple(int(v) for v in bbox) if bbox and len(bbox) == 4 else None
|
bbox_t = tuple(int(v) for v in bbox) if bbox and len(bbox) == 4 else None
|
||||||
for i, pt in enumerate(points):
|
for i, pt in enumerate(points):
|
||||||
r = float(radii[i]) if i < len(radii) else 0.0
|
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}"
|
label = "left" if i == 0 else "right" if i == 1 else f"h{i + 1}"
|
||||||
holes.append(
|
holes.append(
|
||||||
HoleDetection(
|
HoleDetection(
|
||||||
center_px=(float(pt[0]), float(pt[1])),
|
center_px=(float(pt[0]), float(pt[1])),
|
||||||
radius_px=r,
|
radius_px=r,
|
||||||
confidence=1.0,
|
confidence=1.0,
|
||||||
bbox=bbox_t,
|
bbox=bbox_t,
|
||||||
label=label,
|
label=label,
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
draw = raw.get("draw_img", draw_img)
|
draw = raw.get("draw_img", draw_img)
|
||||||
return HoleDetectResult(bool(raw.get("status")), str(raw.get("msg") or ""), holes, draw, raw=raw)
|
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:
|
def _detect_by_model(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
|
||||||
try:
|
try:
|
||||||
detector = self._load_yolo()
|
detector = self._load_yolo()
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
return HoleDetectResult(False, f"模型加载失败: {exc}", [], draw_img)
|
return HoleDetectResult(False, f"模型加载失败: {exc}", [], draw_img)
|
||||||
|
|
||||||
conf = float(self.cfg.get("conf", 0.25))
|
conf = float(self.cfg.get("conf", 0.25))
|
||||||
iou = float(self.cfg.get("iou", 0.7))
|
iou = float(self.cfg.get("iou", 0.7))
|
||||||
imgsz = int(self.cfg.get("imgsz", 640))
|
imgsz = int(self.cfg.get("imgsz", 640))
|
||||||
|
|
||||||
dets = detector.detect(image, conf=conf, iou=iou, imgsz=imgsz)
|
dets = detector.detect(image, conf=conf, iou=iou, imgsz=imgsz)
|
||||||
if not dets:
|
if not dets:
|
||||||
return HoleDetectResult(False, "未检测到孔", [], draw_img)
|
return HoleDetectResult(False, "未检测到孔", [], draw_img)
|
||||||
|
|
||||||
holes: list[HoleDetection] = []
|
holes: list[HoleDetection] = []
|
||||||
for d in dets:
|
for d in dets:
|
||||||
x1, y1, x2, y2 = [int(v) for v in d.xyxy]
|
x1, y1, x2, y2 = [int(v) for v in d.xyxy]
|
||||||
cx = 0.5 * (x1 + x2)
|
cx = 0.5 * (x1 + x2)
|
||||||
cy = 0.5 * (y1 + y2)
|
cy = 0.5 * (y1 + y2)
|
||||||
r = 0.5 * max(x2 - x1, y2 - y1)
|
r = 0.5 * max(x2 - x1, y2 - y1)
|
||||||
holes.append(
|
holes.append(
|
||||||
HoleDetection(
|
HoleDetection(
|
||||||
center_px=(float(cx), float(cy)),
|
center_px=(float(cx), float(cy)),
|
||||||
radius_px=float(r),
|
radius_px=float(r),
|
||||||
confidence=float(d.conf),
|
confidence=float(d.conf),
|
||||||
bbox=(x1, y1, x2, y2),
|
bbox=(x1, y1, x2, y2),
|
||||||
cls_id=int(getattr(d, "cls_id", 0)),
|
cls_id=int(getattr(d, "cls_id", 0)),
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
return _finalize_holes(holes, draw_img, raw={"dets": dets}, cfg=self.cfg)
|
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:
|
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
|
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
|
||||||
ksize = int(self.cfg.get("gaussian_ksize", 5))
|
ksize = int(self.cfg.get("gaussian_ksize", 5))
|
||||||
if ksize % 2 == 0:
|
if ksize % 2 == 0:
|
||||||
ksize += 1
|
ksize += 1
|
||||||
blur = cv2.GaussianBlur(gray, (ksize, ksize), float(self.cfg.get("gaussian_sigma", 1.5)))
|
blur = cv2.GaussianBlur(gray, (ksize, ksize), float(self.cfg.get("gaussian_sigma", 1.5)))
|
||||||
|
|
||||||
circles = cv2.HoughCircles(
|
circles = cv2.HoughCircles(
|
||||||
blur,
|
blur,
|
||||||
cv2.HOUGH_GRADIENT,
|
cv2.HOUGH_GRADIENT,
|
||||||
dp=float(self.cfg.get("hough_dp", 1.2)),
|
dp=float(self.cfg.get("hough_dp", 1.2)),
|
||||||
minDist=float(self.cfg.get("hough_min_dist", 40)),
|
minDist=float(self.cfg.get("hough_min_dist", 40)),
|
||||||
param1=float(self.cfg.get("hough_param1", 100)),
|
param1=float(self.cfg.get("hough_param1", 100)),
|
||||||
param2=float(self.cfg.get("hough_param2", 28)),
|
param2=float(self.cfg.get("hough_param2", 28)),
|
||||||
minRadius=int(self.cfg.get("hough_min_radius", 6)),
|
minRadius=int(self.cfg.get("hough_min_radius", 6)),
|
||||||
maxRadius=int(self.cfg.get("hough_max_radius", 45)),
|
maxRadius=int(self.cfg.get("hough_max_radius", 45)),
|
||||||
)
|
)
|
||||||
if circles is None:
|
if circles is None:
|
||||||
return HoleDetectResult(False, "HoughCircles 未检测到孔", [], draw_img)
|
return HoleDetectResult(False, "HoughCircles 未检测到孔", [], draw_img)
|
||||||
|
|
||||||
holes: list[HoleDetection] = []
|
holes: list[HoleDetection] = []
|
||||||
for c in np.asarray(circles[0], dtype=np.float64):
|
for c in np.asarray(circles[0], dtype=np.float64):
|
||||||
cx, cy, r = float(c[0]), float(c[1]), float(c[2])
|
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))
|
holes.append(HoleDetection(center_px=(cx, cy), radius_px=r, confidence=1.0))
|
||||||
return _finalize_holes(holes, draw_img, cfg=self.cfg)
|
return _finalize_holes(holes, draw_img, cfg=self.cfg)
|
||||||
|
|
||||||
def pixel_to_camera(
|
def pixel_to_camera(
|
||||||
self,
|
self,
|
||||||
hole: HoleDetection,
|
hole: HoleDetection,
|
||||||
depth_m: float | None = None,
|
depth_m: float | None = None,
|
||||||
intrinsics: dict[str, float] | None = None,
|
intrinsics: dict[str, float] | None = None,
|
||||||
) -> np.ndarray:
|
) -> np.ndarray:
|
||||||
depth = float(depth_m if depth_m is not None else self.cfg.get("default_depth_m", 0.10))
|
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 {}
|
intr = intrinsics or self.cfg.get("camera_intrinsics") or {}
|
||||||
return pixel_to_camera(hole, depth, intr)
|
return pixel_to_camera(hole, depth, intr)
|
||||||
|
|
||||||
|
|
||||||
def pair_two_holes(holes: list[HoleDetection], cfg: dict[str, Any] | None = None) -> list[HoleDetection]:
|
def pair_two_holes(holes: list[HoleDetection], cfg: dict[str, Any] | None = None) -> list[HoleDetection]:
|
||||||
"""取左右两孔：优先最高置信度，再按图像 x 排序并打 left/right 标签。"""
|
"""取左右两孔：优先最高置信度，再按图像 x 排序并打 left/right 标签。"""
|
||||||
cfg = cfg or {}
|
cfg = cfg or {}
|
||||||
expected = int(cfg.get("expected_holes", 2))
|
expected = int(cfg.get("expected_holes", 2))
|
||||||
if expected != 2:
|
if expected != 2:
|
||||||
ordered = sorted(holes, key=lambda h: h.center_px[0])
|
ordered = sorted(holes, key=lambda h: h.center_px[0])
|
||||||
for i, hole in enumerate(ordered):
|
for i, hole in enumerate(ordered):
|
||||||
hole.label = f"h{i + 1}"
|
hole.label = f"h{i + 1}"
|
||||||
return ordered
|
return ordered
|
||||||
|
|
||||||
if len(holes) < 2:
|
if len(holes) < 2:
|
||||||
return holes
|
return holes
|
||||||
|
|
||||||
picked = sorted(holes, key=lambda h: h.confidence, reverse=True)
|
picked = sorted(holes, key=lambda h: h.confidence, reverse=True)
|
||||||
if len(picked) > 2:
|
if len(picked) > 2:
|
||||||
by_x = sorted(picked, key=lambda h: h.center_px[0])
|
by_x = sorted(picked, key=lambda h: h.center_px[0])
|
||||||
picked = [by_x[0], by_x[-1]]
|
picked = [by_x[0], by_x[-1]]
|
||||||
else:
|
else:
|
||||||
picked = picked[:2]
|
picked = picked[:2]
|
||||||
picked.sort(key=lambda h: h.center_px[0])
|
picked.sort(key=lambda h: h.center_px[0])
|
||||||
dist = float(
|
dist = float(
|
||||||
np.hypot(
|
np.hypot(
|
||||||
picked[1].center_px[0] - picked[0].center_px[0],
|
picked[1].center_px[0] - picked[0].center_px[0],
|
||||||
picked[1].center_px[1] - picked[0].center_px[1],
|
picked[1].center_px[1] - picked[0].center_px[1],
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
min_sep = float(cfg.get("min_hole_sep_px", 20.0))
|
min_sep = float(cfg.get("min_hole_sep_px", 20.0))
|
||||||
max_sep = float(cfg.get("max_hole_sep_px", 2000.0))
|
max_sep = float(cfg.get("max_hole_sep_px", 2000.0))
|
||||||
if not (min_sep <= dist <= max_sep):
|
if not (min_sep <= dist <= max_sep):
|
||||||
return []
|
return []
|
||||||
picked[0].label = "left"
|
picked[0].label = "left"
|
||||||
picked[1].label = "right"
|
picked[1].label = "right"
|
||||||
return picked
|
return picked
|
||||||
|
|
||||||
|
|
||||||
def _draw_holes(draw_img: np.ndarray, holes: list[HoleDetection]) -> None:
|
def _draw_holes(draw_img: np.ndarray, holes: list[HoleDetection]) -> None:
|
||||||
for hole in holes:
|
for hole in holes:
|
||||||
cx, cy = int(round(hole.center_px[0])), int(round(hole.center_px[1]))
|
cx, cy = int(round(hole.center_px[0])), int(round(hole.center_px[1]))
|
||||||
rr = max(4, int(round(hole.radius_px)))
|
rr = max(4, int(round(hole.radius_px)))
|
||||||
cv2.circle(draw_img, (cx, cy), rr, (0, 0, 255), 2)
|
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.drawMarker(draw_img, (cx, cy), (0, 255, 255), cv2.MARKER_CROSS, 16, 2)
|
||||||
cv2.circle(draw_img, (cx, cy), 3, (0, 255, 255), -1)
|
cv2.circle(draw_img, (cx, cy), 3, (0, 255, 255), -1)
|
||||||
name = hole.label or "hole"
|
name = hole.label or "hole"
|
||||||
cv2.putText(
|
cv2.putText(
|
||||||
draw_img,
|
draw_img,
|
||||||
f"{name} ({cx},{cy}) {hole.confidence:.2f}",
|
f"{name} ({cx},{cy}) {hole.confidence:.2f}",
|
||||||
(cx + 12, cy - 8),
|
(cx + 12, cy - 8),
|
||||||
cv2.FONT_HERSHEY_SIMPLEX,
|
cv2.FONT_HERSHEY_SIMPLEX,
|
||||||
0.55,
|
0.55,
|
||||||
(0, 0, 255),
|
(0, 0, 255),
|
||||||
2,
|
2,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
def _finalize_holes(
|
def _finalize_holes(
|
||||||
holes: list[HoleDetection],
|
holes: list[HoleDetection],
|
||||||
draw_img: np.ndarray,
|
draw_img: np.ndarray,
|
||||||
raw: dict[str, Any] | None = None,
|
raw: dict[str, Any] | None = None,
|
||||||
cfg: dict[str, Any] | None = None,
|
cfg: dict[str, Any] | None = None,
|
||||||
) -> HoleDetectResult:
|
) -> HoleDetectResult:
|
||||||
cfg = cfg or {}
|
cfg = cfg or {}
|
||||||
paired = pair_two_holes(holes, cfg)
|
paired = pair_two_holes(holes, cfg)
|
||||||
if len(holes) >= 2 and not paired:
|
if len(holes) >= 2 and not paired:
|
||||||
_draw_holes(draw_img, holes)
|
_draw_holes(draw_img, holes)
|
||||||
return HoleDetectResult(False, "两孔间距不在允许范围", holes, draw_img, raw=raw)
|
return HoleDetectResult(False, "两孔间距不在允许范围", holes, draw_img, raw=raw)
|
||||||
if not paired:
|
if not paired:
|
||||||
_draw_holes(draw_img, holes)
|
_draw_holes(draw_img, holes)
|
||||||
return HoleDetectResult(False, f"有效孔数量={len(holes)}，需要 2 个", holes, draw_img, raw=raw)
|
return HoleDetectResult(False, f"有效孔数量={len(holes)}，需要 2 个", holes, draw_img, raw=raw)
|
||||||
_draw_holes(draw_img, paired)
|
_draw_holes(draw_img, paired)
|
||||||
dist = float(
|
dist = float(
|
||||||
np.hypot(
|
np.hypot(
|
||||||
paired[1].center_px[0] - paired[0].center_px[0],
|
paired[1].center_px[0] - paired[0].center_px[0],
|
||||||
paired[1].center_px[1] - paired[0].center_px[1],
|
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"
|
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)
|
return HoleDetectResult(True, msg, paired, draw_img, raw=raw)
|
||||||
|
|
||||||
|
|
||||||
def pixel_to_camera(
|
def pixel_to_camera(
|
||||||
hole: HoleDetection,
|
hole: HoleDetection,
|
||||||
depth_m: float,
|
depth_m: float,
|
||||||
intrinsics: dict[str, float],
|
intrinsics: dict[str, float],
|
||||||
) -> np.ndarray:
|
) -> np.ndarray:
|
||||||
"""像素坐标 + 深度 -> 相机系三维坐标（m）。"""
|
"""像素坐标 + 深度 -> 相机系三维坐标（m）。"""
|
||||||
fx = float(intrinsics["fx"])
|
fx = float(intrinsics["fx"])
|
||||||
fy = float(intrinsics["fy"])
|
fy = float(intrinsics["fy"])
|
||||||
cx = float(intrinsics["cx"])
|
cx = float(intrinsics["cx"])
|
||||||
cy = float(intrinsics["cy"])
|
cy = float(intrinsics["cy"])
|
||||||
u, v = hole.center_px
|
u, v = hole.center_px
|
||||||
z = float(depth_m)
|
z = float(depth_m)
|
||||||
x = (u - cx) * z / fx
|
x = (u - cx) * z / fx
|
||||||
y = (v - cy) * z / fy
|
y = (v - cy) * z / fy
|
||||||
return np.array([x, y, z], dtype=float)
|
return np.array([x, y, z], dtype=float)
|
||||||
|
|||||||
@@ -1,42 +1,42 @@
|
|||||||
"""供应用节点加载 ``~/ros2/config/site.yaml``。"""
|
"""供应用节点加载 ``~/ros2/config/site.yaml``。"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import os
|
import os
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from typing import Any
|
from typing import Any
|
||||||
|
|
||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
|
|
||||||
def ros2_root() -> Path:
|
def ros2_root() -> Path:
|
||||||
for key in ("SITE_ROS2_ROOT", "ROS2_WS"):
|
for key in ("SITE_ROS2_ROOT", "ROS2_WS"):
|
||||||
v = os.environ.get(key, "").strip()
|
v = os.environ.get(key, "").strip()
|
||||||
if v:
|
if v:
|
||||||
return Path(v).expanduser()
|
return Path(v).expanduser()
|
||||||
return Path.home() / "ros2"
|
return Path.home() / "ros2"
|
||||||
|
|
||||||
|
|
||||||
def site_path() -> Path:
|
def site_path() -> Path:
|
||||||
env = os.environ.get("SITE_CONFIG", "").strip()
|
env = os.environ.get("SITE_CONFIG", "").strip()
|
||||||
if env:
|
if env:
|
||||||
return Path(env).expanduser()
|
return Path(env).expanduser()
|
||||||
return ros2_root() / "config" / "site.yaml"
|
return ros2_root() / "config" / "site.yaml"
|
||||||
|
|
||||||
|
|
||||||
def load_site_dict(path: str | Path | None = None) -> dict[str, Any]:
|
def load_site_dict(path: str | Path | None = None) -> dict[str, Any]:
|
||||||
p = Path(path).expanduser() if path else site_path()
|
p = Path(path).expanduser() if path else site_path()
|
||||||
if not p.is_file():
|
if not p.is_file():
|
||||||
return {}
|
return {}
|
||||||
data = yaml.safe_load(p.read_text(encoding="utf-8")) or {}
|
data = yaml.safe_load(p.read_text(encoding="utf-8")) or {}
|
||||||
return data if isinstance(data, dict) else {}
|
return data if isinstance(data, dict) else {}
|
||||||
|
|
||||||
|
|
||||||
def deep_merge(base: dict[str, Any], overlay: dict[str, Any]) -> dict[str, Any]:
|
def deep_merge(base: dict[str, Any], overlay: dict[str, Any]) -> dict[str, Any]:
|
||||||
out = dict(base)
|
out = dict(base)
|
||||||
for key, val in overlay.items():
|
for key, val in overlay.items():
|
||||||
if isinstance(val, dict) and isinstance(out.get(key), dict):
|
if isinstance(val, dict) and isinstance(out.get(key), dict):
|
||||||
out[key] = deep_merge(out[key], val)
|
out[key] = deep_merge(out[key], val)
|
||||||
else:
|
else:
|
||||||
out[key] = val
|
out[key] = val
|
||||||
return out
|
return out
|
||||||
|
|||||||
@@ -1,60 +1,60 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""Orisys 视触觉命令行工具。端口 / 配置来自 ``site.yaml`` 的 ``tactile`` 段。"""
|
"""Orisys 视触觉命令行工具。端口 / 配置来自 ``site.yaml`` 的 ``tactile`` 段。"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import argparse
|
import argparse
|
||||||
import sys
|
import sys
|
||||||
import time
|
import time
|
||||||
|
|
||||||
from arm_application.orisys_tactile import make_tactile, resolve_orisys_config
|
from arm_application.orisys_tactile import make_tactile, resolve_orisys_config
|
||||||
from arm_application.site_io import load_site_dict, site_path
|
from arm_application.site_io import load_site_dict, site_path
|
||||||
|
|
||||||
|
|
||||||
def main() -> int:
|
def main() -> int:
|
||||||
parser = argparse.ArgumentParser(description="Orisys 触觉命令行工具（site.yaml）")
|
parser = argparse.ArgumentParser(description="Orisys 触觉命令行工具（site.yaml）")
|
||||||
parser.add_argument("--duration", type=float, default=10.0)
|
parser.add_argument("--duration", type=float, default=10.0)
|
||||||
parser.add_argument("--site", default="")
|
parser.add_argument("--site", default="")
|
||||||
args = parser.parse_args()
|
args = parser.parse_args()
|
||||||
data = load_site_dict(args.site or None)
|
data = load_site_dict(args.site or None)
|
||||||
t = data.get("tactile") or {}
|
t = data.get("tactile") or {}
|
||||||
cfg_name = resolve_orisys_config(str(t.get("config") or "dd03"), str(t.get("config_file") or ""))
|
cfg_name = resolve_orisys_config(str(t.get("config") or "dd03"), str(t.get("config_file") or ""))
|
||||||
print(
|
print(
|
||||||
f"site={site_path() if not args.site else args.site} "
|
f"site={site_path() if not args.site else args.site} "
|
||||||
f"backend={t.get('backend') or 'orisys'} "
|
f"backend={t.get('backend') or 'orisys'} "
|
||||||
f"left={t.get('left_video', 0)} right={t.get('right_video', 2)} "
|
f"left={t.get('left_video', 0)} right={t.get('right_video', 2)} "
|
||||||
f"config={cfg_name}"
|
f"config={cfg_name}"
|
||||||
)
|
)
|
||||||
tactile = make_tactile(data)
|
tactile = make_tactile(data)
|
||||||
try:
|
try:
|
||||||
tactile.connect()
|
tactile.connect()
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
print(f"错误：{exc}", file=sys.stderr)
|
print(f"错误：{exc}", file=sys.stderr)
|
||||||
return 1
|
return 1
|
||||||
t0 = time.time()
|
t0 = time.time()
|
||||||
n = 0
|
n = 0
|
||||||
try:
|
try:
|
||||||
while time.time() - t0 < args.duration:
|
while time.time() - t0 < args.duration:
|
||||||
sample = tactile.read_sample()
|
sample = tactile.read_sample()
|
||||||
n += 1
|
n += 1
|
||||||
raw = sample.raw or {}
|
raw = sample.raw or {}
|
||||||
left = raw.get("left") or {}
|
left = raw.get("left") or {}
|
||||||
right = raw.get("right") or {}
|
right = raw.get("right") or {}
|
||||||
if n % 10 == 0:
|
if n % 10 == 0:
|
||||||
print(
|
print(
|
||||||
f"n={n:4d} contact={int(sample.contact)} "
|
f"n={n:4d} contact={int(sample.contact)} "
|
||||||
f"L F={float(left.get('fnormal') or 0):8.2f} "
|
f"L F={float(left.get('fnormal') or 0):8.2f} "
|
||||||
f"R F={float(right.get('fnormal') or 0):8.2f} "
|
f"R F={float(right.get('fnormal') or 0):8.2f} "
|
||||||
f"Fz={float(sample.force_xyz[0]):8.2f}"
|
f"Fz={float(sample.force_xyz[0]):8.2f}"
|
||||||
)
|
)
|
||||||
time.sleep(0.01)
|
time.sleep(0.01)
|
||||||
except KeyboardInterrupt:
|
except KeyboardInterrupt:
|
||||||
print("已中断")
|
print("已中断")
|
||||||
finally:
|
finally:
|
||||||
tactile.disconnect()
|
tactile.disconnect()
|
||||||
print(f"完成 frames={n}")
|
print(f"完成 frames={n}")
|
||||||
return 0
|
return 0
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
raise SystemExit(main())
|
raise SystemExit(main())
|
||||||
|
|||||||
@@ -1,84 +1,84 @@
|
|||||||
"""触觉采样 / 指令类型。后端（Orisys / mock）可替换。"""
|
"""触觉采样 / 指令类型。后端（Orisys / mock）可替换。"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import time
|
import time
|
||||||
from abc import ABC, abstractmethod
|
from abc import ABC, abstractmethod
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
from typing import Any
|
from typing import Any
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class TactileSample:
|
class TactileSample:
|
||||||
timestamp: float
|
timestamp: float
|
||||||
force_xyz: np.ndarray
|
force_xyz: np.ndarray
|
||||||
torque_xyz: np.ndarray
|
torque_xyz: np.ndarray
|
||||||
contact: bool
|
contact: bool
|
||||||
embedding: np.ndarray | None = None
|
embedding: np.ndarray | None = None
|
||||||
raw: dict[str, Any] | None = None
|
raw: dict[str, Any] | None = None
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class TactileCommand:
|
class TactileCommand:
|
||||||
delta_xyz: np.ndarray
|
delta_xyz: np.ndarray
|
||||||
delta_rpy: np.ndarray
|
delta_rpy: np.ndarray
|
||||||
gripper_pos: float | None = None
|
gripper_pos: float | None = None
|
||||||
done: bool = False
|
done: bool = False
|
||||||
ok: bool = True
|
ok: bool = True
|
||||||
message: str = ""
|
message: str = ""
|
||||||
|
|
||||||
|
|
||||||
class TactileInterface(ABC):
|
class TactileInterface(ABC):
|
||||||
name: str = "tactile_base"
|
name: str = "tactile_base"
|
||||||
|
|
||||||
@abstractmethod
|
@abstractmethod
|
||||||
def connect(self) -> None: ...
|
def connect(self) -> None: ...
|
||||||
|
|
||||||
@abstractmethod
|
@abstractmethod
|
||||||
def disconnect(self) -> None: ...
|
def disconnect(self) -> None: ...
|
||||||
|
|
||||||
@abstractmethod
|
@abstractmethod
|
||||||
def reset(self, hole_id: int) -> None: ...
|
def reset(self, hole_id: int) -> None: ...
|
||||||
|
|
||||||
@abstractmethod
|
@abstractmethod
|
||||||
def step(self, sample: TactileSample) -> TactileCommand: ...
|
def step(self, sample: TactileSample) -> TactileCommand: ...
|
||||||
|
|
||||||
|
|
||||||
class MockTactile(TactileInterface):
|
class MockTactile(TactileInterface):
|
||||||
name = "mock_tactile"
|
name = "mock_tactile"
|
||||||
|
|
||||||
def __init__(self) -> None:
|
def __init__(self) -> None:
|
||||||
self._i = 0
|
self._i = 0
|
||||||
self._hole = 0
|
self._hole = 0
|
||||||
|
|
||||||
def connect(self) -> None:
|
def connect(self) -> None:
|
||||||
self._i = 0
|
self._i = 0
|
||||||
|
|
||||||
def disconnect(self) -> None:
|
def disconnect(self) -> None:
|
||||||
pass
|
pass
|
||||||
|
|
||||||
def reset(self, hole_id: int) -> None:
|
def reset(self, hole_id: int) -> None:
|
||||||
self._hole = hole_id
|
self._hole = hole_id
|
||||||
self._i = 0
|
self._i = 0
|
||||||
|
|
||||||
def read_sample(self) -> TactileSample:
|
def read_sample(self) -> TactileSample:
|
||||||
self._i += 1
|
self._i += 1
|
||||||
return TactileSample(
|
return TactileSample(
|
||||||
timestamp=time.time(),
|
timestamp=time.time(),
|
||||||
force_xyz=np.zeros(3),
|
force_xyz=np.zeros(3),
|
||||||
torque_xyz=np.zeros(3),
|
torque_xyz=np.zeros(3),
|
||||||
contact=self._i >= 8,
|
contact=self._i >= 8,
|
||||||
)
|
)
|
||||||
|
|
||||||
def step(self, sample: TactileSample) -> TactileCommand:
|
def step(self, sample: TactileSample) -> TactileCommand:
|
||||||
self._i += 1
|
self._i += 1
|
||||||
done = self._i >= 15
|
done = self._i >= 15
|
||||||
return TactileCommand(
|
return TactileCommand(
|
||||||
delta_xyz=np.zeros(3),
|
delta_xyz=np.zeros(3),
|
||||||
delta_rpy=np.zeros(3),
|
delta_rpy=np.zeros(3),
|
||||||
done=done,
|
done=done,
|
||||||
ok=True,
|
ok=True,
|
||||||
message=f"mock hole={self._hole} step={self._i}",
|
message=f"mock hole={self._hole} step={self._i}",
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -1,25 +1,25 @@
|
|||||||
# DualArmGripperManager(load_dual_arm_gripper_config(path)) 的可选兜底配置。
|
# DualArmGripperManager(load_dual_arm_gripper_config(path)) 的可选兜底配置。
|
||||||
# 日常使用通过 load_from_site() 读取 ~/ros2/config/site.yaml。
|
# 日常使用通过 load_from_site() 读取 ~/ros2/config/site.yaml。
|
||||||
left_arm:
|
left_arm:
|
||||||
robot_sn: "Rizon4-063989"
|
robot_sn: "Rizon4-063989"
|
||||||
gripper_port: "/dev/ttyUSB0"
|
gripper_port: "/dev/ttyUSB0"
|
||||||
gripper_slave_id: 1
|
gripper_slave_id: 1
|
||||||
gripper_baudrate: 115200
|
gripper_baudrate: 115200
|
||||||
right_arm:
|
right_arm:
|
||||||
robot_sn: "Rizon4-063991"
|
robot_sn: "Rizon4-063991"
|
||||||
gripper_port: "/dev/ttyUSB1"
|
gripper_port: "/dev/ttyUSB1"
|
||||||
gripper_slave_id: 1
|
gripper_slave_id: 1
|
||||||
gripper_baudrate: 115200
|
gripper_baudrate: 115200
|
||||||
gripper:
|
gripper:
|
||||||
parallel:
|
parallel:
|
||||||
stroke_min: 0.0
|
stroke_min: 0.0
|
||||||
stroke_max: 20.0
|
stroke_max: 20.0
|
||||||
default_speed: 50
|
default_speed: 50
|
||||||
default_force: 20
|
default_force: 20
|
||||||
auto_initialize: true
|
auto_initialize: true
|
||||||
rotary:
|
rotary:
|
||||||
auto_initialize: false
|
auto_initialize: false
|
||||||
default_speed: 50
|
default_speed: 50
|
||||||
default_torque: 50
|
default_torque: 50
|
||||||
finger_open_per_mille: 1000
|
finger_open_per_mille: 1000
|
||||||
finger_close_per_mille: 0
|
finger_close_per_mille: 0
|
||||||
|
|||||||
@@ -1,37 +1,37 @@
|
|||||||
# 孔定位检测配置（改 conf / 内参 / 双孔间距即可，不必改代码）
|
# 孔定位检测配置（改 conf / 内参 / 双孔间距即可，不必改代码）
|
||||||
# 权重来源：优先 site.yaml（hole_detect.weights / paths.hole_weights）；
|
# 权重来源：优先 site.yaml（hole_detect.weights / paths.hole_weights）；
|
||||||
# 两者都缺时兜底到 models/hole_insert/detect_best_1.onnx。
|
# 两者都缺时兜底到 models/hole_insert/detect_best_1.onnx。
|
||||||
# 原先这里注释写的是 best.pt —— 该文件不存在，已修正。
|
# 原先这里注释写的是 best.pt —— 该文件不存在，已修正。
|
||||||
# backend: yolo | hough | two_stage（two_stage 请用 hole_detect.yaml）
|
# backend: yolo | hough | two_stage（two_stage 请用 hole_detect.yaml）
|
||||||
|
|
||||||
model_path: ""
|
model_path: ""
|
||||||
backend: yolo # yolo | faster_rcnn | hough
|
backend: yolo # yolo | faster_rcnn | hough
|
||||||
num_classes: 1 # 单类孔=1；左右孔分两类=2
|
num_classes: 1 # 单类孔=1；左右孔分两类=2
|
||||||
expected_holes: 2
|
expected_holes: 2
|
||||||
min_hole_sep_px: 20
|
min_hole_sep_px: 20
|
||||||
max_hole_sep_px: 2000
|
max_hole_sep_px: 2000
|
||||||
|
|
||||||
conf: 0.25
|
conf: 0.25
|
||||||
iou: 0.7
|
iou: 0.7
|
||||||
imgsz: 640
|
imgsz: 640
|
||||||
|
|
||||||
gaussian_ksize: 5
|
gaussian_ksize: 5
|
||||||
gaussian_sigma: 1.5
|
gaussian_sigma: 1.5
|
||||||
hough_dp: 1.2
|
hough_dp: 1.2
|
||||||
hough_min_dist: 40
|
hough_min_dist: 40
|
||||||
hough_param1: 100
|
hough_param1: 100
|
||||||
hough_param2: 28
|
hough_param2: 28
|
||||||
hough_min_radius: 6
|
hough_min_radius: 6
|
||||||
hough_max_radius: 45
|
hough_max_radius: 45
|
||||||
|
|
||||||
stabilize_window: 7
|
stabilize_window: 7
|
||||||
stabilize_ema: 0.35
|
stabilize_ema: 0.35
|
||||||
stabilize_max_jump_px: 60
|
stabilize_max_jump_px: 60
|
||||||
|
|
||||||
camera_intrinsics:
|
camera_intrinsics:
|
||||||
fx: 425.0
|
fx: 425.0
|
||||||
fy: 425.0
|
fy: 425.0
|
||||||
cx: 640.0
|
cx: 640.0
|
||||||
cy: 360.0
|
cy: 360.0
|
||||||
|
|
||||||
default_depth_m: 0.10
|
default_depth_m: 0.10
|
||||||
|
|||||||
@@ -1,40 +1,40 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""启动双路 DH 夹爪。端口来自 ``site.yaml`` 的 ``robots[].gripper``。"""
|
"""启动双路 DH 夹爪。端口来自 ``site.yaml`` 的 ``robots[].gripper``。"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import os
|
import os
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
|
||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch.actions import DeclareLaunchArgument, LogInfo
|
from launch.actions import DeclareLaunchArgument, LogInfo
|
||||||
from launch.substitutions import LaunchConfiguration
|
from launch.substitutions import LaunchConfiguration
|
||||||
from launch_ros.actions import Node
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description() -> LaunchDescription:
|
def generate_launch_description() -> LaunchDescription:
|
||||||
sides = LaunchConfiguration("sides")
|
sides = LaunchConfiguration("sides")
|
||||||
allow_offline = LaunchConfiguration("allow_offline")
|
allow_offline = LaunchConfiguration("allow_offline")
|
||||||
site = os.environ.get("SITE_CONFIG") or str(Path.home() / "ros2" / "config" / "site.yaml")
|
site = os.environ.get("SITE_CONFIG") or str(Path.home() / "ros2" / "config" / "site.yaml")
|
||||||
return LaunchDescription(
|
return LaunchDescription(
|
||||||
[
|
[
|
||||||
DeclareLaunchArgument("sides", default_value="left,right"),
|
DeclareLaunchArgument("sides", default_value="left,right"),
|
||||||
DeclareLaunchArgument("allow_offline", default_value="true"),
|
DeclareLaunchArgument("allow_offline", default_value="true"),
|
||||||
LogInfo(msg=f"[dual_gripper] site={site}（arm_msgs，IPC Modbus，不是 Flexiv 夹爪）"),
|
LogInfo(msg=f"[dual_gripper] site={site}（arm_msgs，IPC Modbus，不是 Flexiv 夹爪）"),
|
||||||
Node(
|
Node(
|
||||||
package="arm_application",
|
package="arm_application",
|
||||||
executable="dual_gripper",
|
executable="dual_gripper",
|
||||||
name="dual_gripper_driver",
|
name="dual_gripper_driver",
|
||||||
output="screen",
|
output="screen",
|
||||||
parameters=[
|
parameters=[
|
||||||
{
|
{
|
||||||
"config_file": "",
|
"config_file": "",
|
||||||
"auto_connect": True,
|
"auto_connect": True,
|
||||||
"allow_offline": allow_offline,
|
"allow_offline": allow_offline,
|
||||||
"publish_hz": 10.0,
|
"publish_hz": 10.0,
|
||||||
"sides": sides,
|
"sides": sides,
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
),
|
),
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -1,70 +1,70 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""在当前机械臂的彩色话题上启动 hole_detect。"""
|
"""在当前机械臂的彩色话题上启动 hole_detect。"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import os
|
import os
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
|
||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
|
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
|
||||||
from launch.substitutions import LaunchConfiguration
|
from launch.substitutions import LaunchConfiguration
|
||||||
from launch_ros.actions import Node
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
|
||||||
def _ws_root() -> Path:
|
def _ws_root() -> Path:
|
||||||
for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
|
for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
|
||||||
v = os.environ.get(key, "").strip()
|
v = os.environ.get(key, "").strip()
|
||||||
if v:
|
if v:
|
||||||
return Path(v)
|
return Path(v)
|
||||||
return Path.home() / "ros2"
|
return Path.home() / "ros2"
|
||||||
|
|
||||||
|
|
||||||
def _setup(context, *args, **kwargs):
|
def _setup(context, *args, **kwargs):
|
||||||
root = _ws_root()
|
root = _ws_root()
|
||||||
backend = LaunchConfiguration("backend").perform(context) or os.environ.get("SITE_HOLE_BACKEND") or "two_stage"
|
backend = LaunchConfiguration("backend").perform(context) or os.environ.get("SITE_HOLE_BACKEND") or "two_stage"
|
||||||
weights = LaunchConfiguration("weights").perform(context) or os.environ.get("SITE_HOLE_WEIGHTS") or ""
|
weights = LaunchConfiguration("weights").perform(context) or os.environ.get("SITE_HOLE_WEIGHTS") or ""
|
||||||
cfg = LaunchConfiguration("config_file").perform(context) or os.environ.get("SITE_HOLE_CONFIG") or ""
|
cfg = LaunchConfiguration("config_file").perform(context) or os.environ.get("SITE_HOLE_CONFIG") or ""
|
||||||
cam = os.environ.get("SITE_CAM_NAME") or "d405"
|
cam = os.environ.get("SITE_CAM_NAME") or "d405"
|
||||||
preview = LaunchConfiguration("preview").perform(context).lower() in ("1", "true", "yes")
|
preview = LaunchConfiguration("preview").perform(context).lower() in ("1", "true", "yes")
|
||||||
if not cfg:
|
if not cfg:
|
||||||
if backend in ("two_stage", "hough_roi"):
|
if backend in ("two_stage", "hough_roi"):
|
||||||
cfg = str(root / "src" / "arm_application" / "config" / "hole_detect.yaml")
|
cfg = str(root / "src" / "arm_application" / "config" / "hole_detect.yaml")
|
||||||
else:
|
else:
|
||||||
cfg = str(root / "src" / "arm_application" / "config" / "hole_insert.yaml")
|
cfg = str(root / "src" / "arm_application" / "config" / "hole_insert.yaml")
|
||||||
if not weights:
|
if not weights:
|
||||||
if backend in ("two_stage", "hough_roi"):
|
if backend in ("two_stage", "hough_roi"):
|
||||||
weights = os.environ.get("SITE_HOLE_WORKPIECE_WEIGHTS") or os.environ.get("SITE_YOLO_WEIGHTS") or ""
|
weights = os.environ.get("SITE_HOLE_WORKPIECE_WEIGHTS") or os.environ.get("SITE_YOLO_WEIGHTS") or ""
|
||||||
else:
|
else:
|
||||||
weights = os.environ.get("SITE_HOLE_WEIGHTS") or str(root / "models" / "hole_insert" / "detect_best_1.onnx")
|
weights = os.environ.get("SITE_HOLE_WEIGHTS") or str(root / "models" / "hole_insert" / "detect_best_1.onnx")
|
||||||
return [
|
return [
|
||||||
LogInfo(msg=f"[hole_detect] cam={cam} backend={backend} weights={weights} cfg={cfg}"),
|
LogInfo(msg=f"[hole_detect] cam={cam} backend={backend} weights={weights} cfg={cfg}"),
|
||||||
Node(
|
Node(
|
||||||
package="arm_application",
|
package="arm_application",
|
||||||
executable="hole_detect",
|
executable="hole_detect",
|
||||||
name="hole_detect",
|
name="hole_detect",
|
||||||
output="screen",
|
output="screen",
|
||||||
parameters=[
|
parameters=[
|
||||||
{
|
{
|
||||||
"camera_source": cam,
|
"camera_source": cam,
|
||||||
"weights": weights,
|
"weights": weights,
|
||||||
"config_file": cfg,
|
"config_file": cfg,
|
||||||
"backend": backend,
|
"backend": backend,
|
||||||
"device": os.environ.get("SITE_YOLO_DEVICE") or "0",
|
"device": os.environ.get("SITE_YOLO_DEVICE") or "0",
|
||||||
"preview": preview,
|
"preview": preview,
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
),
|
),
|
||||||
]
|
]
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description() -> LaunchDescription:
|
def generate_launch_description() -> LaunchDescription:
|
||||||
return LaunchDescription(
|
return LaunchDescription(
|
||||||
[
|
[
|
||||||
DeclareLaunchArgument("backend", default_value=""),
|
DeclareLaunchArgument("backend", default_value=""),
|
||||||
DeclareLaunchArgument("weights", default_value=""),
|
DeclareLaunchArgument("weights", default_value=""),
|
||||||
DeclareLaunchArgument("config_file", default_value=""),
|
DeclareLaunchArgument("config_file", default_value=""),
|
||||||
DeclareLaunchArgument("preview", default_value="false"),
|
DeclareLaunchArgument("preview", default_value="false"),
|
||||||
OpaqueFunction(function=_setup),
|
OpaqueFunction(function=_setup),
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -1,16 +1,16 @@
|
|||||||
std_msgs/Header header
|
std_msgs/Header header
|
||||||
|
|
||||||
# init|init_full|connect|open|close|stop|move|rotate|rotate_to|rotate_by|rotate_revolutions|home_rotation|set_force|set_speed|set_finger_force|set_finger_speed
|
# init|init_full|connect|open|close|stop|move|rotate|rotate_to|rotate_by|rotate_revolutions|home_rotation|set_force|set_speed|set_finger_force|set_finger_speed
|
||||||
string command
|
string command
|
||||||
|
|
||||||
# command=move: 平行夹爪行程 mm；command=rotate/rotate_by: 角度 deg；command=rotate_revolutions: 圈数
|
# command=move: 平行夹爪行程 mm；command=rotate/rotate_by: 角度 deg；command=rotate_revolutions: 圈数
|
||||||
float64 position
|
float64 position
|
||||||
|
|
||||||
# 速度百分比（1~100）
|
# 速度百分比（1~100）
|
||||||
float64 speed
|
float64 speed
|
||||||
|
|
||||||
# 力/力矩百分比（DH 一般 20~100）
|
# 力/力矩百分比（DH 一般 20~100）
|
||||||
float64 force
|
float64 force
|
||||||
|
|
||||||
bool wait_complete
|
bool wait_complete
|
||||||
float64 timeout
|
float64 timeout
|
||||||
|
|||||||
@@ -1,25 +1,25 @@
|
|||||||
std_msgs/Header header
|
std_msgs/Header header
|
||||||
|
|
||||||
string name
|
string name
|
||||||
string gripper_type # parallel|rotary
|
string gripper_type # parallel|rotary
|
||||||
string state # IDLE|MOVING|GRIPPING|RELEASING|ERROR
|
string state # IDLE|MOVING|GRIPPING|RELEASING|ERROR
|
||||||
|
|
||||||
bool connected
|
bool connected
|
||||||
bool initialized
|
bool initialized
|
||||||
bool is_moving
|
bool is_moving
|
||||||
bool is_gripped
|
bool is_gripped
|
||||||
|
|
||||||
# 平行夹爪：当前行程 mm；旋转夹爪：当前旋转角度 deg
|
# 平行夹爪：当前行程 mm；旋转夹爪：当前旋转角度 deg
|
||||||
float64 position
|
float64 position
|
||||||
|
|
||||||
# RGI 旋转夹爪额外反馈；平行夹爪为 0
|
# RGI 旋转夹爪额外反馈；平行夹爪为 0
|
||||||
float64 finger_position_per_mille
|
float64 finger_position_per_mille
|
||||||
float64 rotation_deg
|
float64 rotation_deg
|
||||||
|
|
||||||
# DH 原始状态字；未读到为 -1
|
# DH 原始状态字；未读到为 -1
|
||||||
int32 gripper_state # 0 moving, 1 reached, 2 object caught, 3 object dropped
|
int32 gripper_state # 0 moving, 1 reached, 2 object caught, 3 object dropped
|
||||||
int32 rotation_state # RGI: 0 moving, 1 reached, 2 blocking, 3 had been blocked
|
int32 rotation_state # RGI: 0 moving, 1 reached, 2 blocking, 3 had been blocked
|
||||||
|
|
||||||
float64 velocity # 最近一次命令速度 %
|
float64 velocity # 最近一次命令速度 %
|
||||||
float64 force # 最近一次命令力/力矩 %
|
float64 force # 最近一次命令力/力矩 %
|
||||||
string error_msg
|
string error_msg
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
float32 u
|
float32 u
|
||||||
float32 v
|
float32 v
|
||||||
float32 radius_px
|
float32 radius_px
|
||||||
float32 confidence
|
float32 confidence
|
||||||
string label
|
string label
|
||||||
int32[] bbox
|
int32[] bbox
|
||||||
|
|||||||
@@ -1,4 +1,4 @@
|
|||||||
std_msgs/Header header
|
std_msgs/Header header
|
||||||
bool ok
|
bool ok
|
||||||
string message
|
string message
|
||||||
arm_msgs/HoleDetection[] holes
|
arm_msgs/HoleDetection[] holes
|
||||||
|
|||||||
@@ -1,11 +1,11 @@
|
|||||||
std_msgs/Header header
|
std_msgs/Header header
|
||||||
string side
|
string side
|
||||||
bool connected
|
bool connected
|
||||||
bool contact
|
bool contact
|
||||||
float32 fnormal
|
float32 fnormal
|
||||||
float32 fshearx
|
float32 fshearx
|
||||||
float32 fsheary
|
float32 fsheary
|
||||||
float32 fps
|
float32 fps
|
||||||
float32 centroid_u
|
float32 centroid_u
|
||||||
float32 centroid_v
|
float32 centroid_v
|
||||||
string error_msg
|
string error_msg
|
||||||
|
|||||||
@@ -1,5 +1,5 @@
|
|||||||
arm_msgs/GripperCommand command
|
arm_msgs/GripperCommand command
|
||||||
---
|
---
|
||||||
bool success
|
bool success
|
||||||
string message
|
string message
|
||||||
arm_msgs/GripperStatus status
|
arm_msgs/GripperStatus status
|
||||||
|
|||||||
Reference in New Issue
Block a user