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