diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..0869827 --- /dev/null +++ b/.gitattributes @@ -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 diff --git a/models/hole_insert/README.md b/models/hole_insert/README.md index 7aaeabb..8f84f5d 100644 --- a/models/hole_insert/README.md +++ b/models/hole_insert/README.md @@ -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 +``` diff --git a/scripts/run_dual_gripper.sh b/scripts/run_dual_gripper.sh index b31e436..e41077f 100755 --- a/scripts/run_dual_gripper.sh +++ b/scripts/run_dual_gripper.sh @@ -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 diff --git a/scripts/run_hole_detect.sh b/scripts/run_hole_detect.sh index f4d2915..0e98858 100755 --- a/scripts/run_hole_detect.sh +++ b/scripts/run_hole_detect.sh @@ -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}" diff --git a/src/arm_application/arm_application/dual_gripper.py b/src/arm_application/arm_application/dual_gripper.py index 4c7708b..511831d 100644 --- a/src/arm_application/arm_application/dual_gripper.py +++ b/src/arm_application/arm_application/dual_gripper.py @@ -1,803 +1,803 @@ -"""双臂夹爪统一控制架构（DH-Robotics 官方 Modbus RTU，非 Flexiv RDK）。 - -设计目标： -- 左臂（Rizon4-063989）：大寰 PGEA 平行夹爪，控制行程/夹持力/速度。 -- 右臂（Rizon4-063991）：大寰 RGI-100-30-O-S-M1 旋转夹爪，控制手指夹持与旋转角度/圈数/速度。 -- 统一接口，便于后续扩展到 ROS2 节点和新项目脚本。 - -注意：DH 官方协议是“写位置/角度寄存器后立即运动”，没有 ChangingTek SDK 的 0x0108 触发寄存器。 -""" - -from __future__ import annotations - -import time -from abc import ABC, abstractmethod -from dataclasses import dataclass, field -from enum import Enum, auto -from pathlib import Path -from typing import Any - -import numpy as np - -from arm_application.dh_official import DHModbusClient, DHModbusError - - -class GripperType(Enum): - PARALLEL = auto() # 平行夹爪 - ROTARY = auto() # 旋转夹爪 - VACUUM = auto() # 真空吸盘（预留） - - -class GripperState(Enum): - IDLE = auto() - MOVING = auto() - GRIPPING = auto() - RELEASING = auto() - ERROR = auto() - - -@dataclass -class GripperStatus: - """夹爪状态反馈。""" - - state: GripperState = GripperState.IDLE - position: float = 0.0 # 平行：mm；旋转：角度 deg - force: float = 0.0 # 最近一次命令力/力矩 % - velocity: float = 0.0 # 最近一次命令速度 % - is_moving: bool = False - is_gripped: bool = False # 是否夹持到物体 - connected: bool = False - initialized: bool = False - finger_position_per_mille: float = 0.0 - rotation_deg: float = 0.0 - gripper_state: int = -1 # DH 0x0201：0 moving,1 reached,2 caught,3 dropped - rotation_state: int = -1 # RGI 0x020B：0 moving,1 reached,2 blocking,3 blocked - error_msg: str = "" - timestamp: float = field(default_factory=time.time) - - -@dataclass -class GripperCommand: - """夹爪控制指令。""" - - target_position: float | None = None # 平行：mm；旋转：deg - target_force: float | None = None # 目标力/力矩 % - target_velocity: float | None = None # 目标速度 % - wait_complete: bool = True # 是否等待完成 - timeout: float = 10.0 # 超时时间 - - -class BaseGripper(ABC): - """夹爪基类，定义统一接口。""" - - name: str = "base_gripper" - gripper_type: GripperType = GripperType.PARALLEL - - def __init__(self, config: dict[str, Any] | None = None) -> None: - self.cfg = config or {} - self._connected = False - self._status = GripperStatus() - - @abstractmethod - def connect(self) -> None: ... - - @abstractmethod - def disconnect(self) -> None: ... - - @abstractmethod - def execute(self, cmd: GripperCommand) -> GripperStatus: ... - - @abstractmethod - def get_status(self) -> GripperStatus: ... - - def is_connected(self) -> bool: - return self._connected - - def open(self, **kwargs) -> GripperStatus: - raise NotImplementedError - - def close(self, **kwargs) -> GripperStatus: - raise NotImplementedError - - def stop(self) -> None: - raise NotImplementedError - - -# ============================================================================= -# 大寰平行夹爪（PGE/PGEA 系列） -# ============================================================================= - -@dataclass -class ParallelGripperConfig: - """大寰平行夹爪配置。""" - - port: str = "/dev/ttyUSB0" - slave_id: int = 1 - baudrate: int = 115200 - timeout: float = 0.2 - - # PGEA-100-40 行程为 40mm；若实际是 PGEA-100-26，请把 stroke_max 改成 26.0。 - stroke_min: float = 0.0 - stroke_max: float = 40.0 - - # 默认参数（DH 百分比） - default_speed: int = 50 - default_force: int = 50 - force_min: int = 20 - force_max: int = 100 - - # 控制行为 - auto_initialize: bool = True - init_timeout: float = 10.0 - position_tolerance_per_mille: int = 15 - status_poll_interval: float = 0.05 - - -class ParallelGripper(BaseGripper): - """大寰 PGE/PGEA 平行夹爪。""" - - name = "dh_pgea_parallel" - gripper_type = GripperType.PARALLEL - - def __init__(self, config: ParallelGripperConfig | None = None) -> None: - super().__init__() - self.cfg = config or ParallelGripperConfig() - self._client: DHModbusClient | None = None - self._last_position_mm = float(self.cfg.stroke_min) - self._last_force = int(self.cfg.default_force) - self._last_speed = int(self.cfg.default_speed) - - def connect(self) -> None: - if self._client is not None: - self.disconnect() - self._client = DHModbusClient( - self.cfg.port, - slave_id=self.cfg.slave_id, - baudrate=self.cfg.baudrate, - timeout=self.cfg.timeout, - ) - self._connected = True - self._status.connected = True - self._status.state = GripperState.IDLE - self._status.error_msg = "" - if self.cfg.auto_initialize: - self.initialize() - - def disconnect(self) -> None: - if self._client is not None: - self._client.close() - self._client = None - self._connected = False - self._status.connected = False - self._status.state = GripperState.IDLE - - def initialize(self, full: bool = False) -> None: - self._require_client() - assert self._client is not None - self._client.initialize(full=full, rotation=False, timeout=self.cfg.init_timeout) - self._status.initialized = True - - def _require_client(self) -> None: - if not self._connected or self._client is None: - self._status.state = GripperState.ERROR - self._status.error_msg = "平行夹爪未连接" - raise DHModbusError(self._status.error_msg) - - def _mm_to_per_mille(self, position_mm: float) -> int: - lo = float(self.cfg.stroke_min) - hi = float(self.cfg.stroke_max) - if hi <= lo: - hi = lo + 1.0 - ratio = (float(position_mm) - lo) / (hi - lo) - return int(np.clip(round(ratio * 1000.0), 0, 1000)) - - def _per_mille_to_mm(self, position: float | int) -> float: - lo = float(self.cfg.stroke_min) - hi = float(self.cfg.stroke_max) - return lo + (float(position) / 1000.0) * (hi - lo) - - def execute(self, cmd: GripperCommand) -> GripperStatus: - try: - self._require_client() - except DHModbusError: - return self._status - assert self._client is not None - - position_mm = self._last_position_mm if cmd.target_position is None else float(cmd.target_position) - position_mm = float(np.clip(position_mm, self.cfg.stroke_min, self.cfg.stroke_max)) - speed = int(np.clip(cmd.target_velocity or self.cfg.default_speed, 1, 100)) - force = int(np.clip(cmd.target_force or self.cfg.default_force, self.cfg.force_min, self.cfg.force_max)) - - self._client.set_speed_pct(speed) - self._client.set_force_pct(force) - target_per_mille = self._client.set_position_per_mille(self._mm_to_per_mille(position_mm)) - - self._last_position_mm = position_mm - self._last_speed = speed - self._last_force = force - self._status.state = GripperState.MOVING - self._status.error_msg = "" - - if cmd.wait_complete: - self._wait_complete(target_per_mille, cmd.timeout) - return self.get_status() - - def _wait_complete(self, target_per_mille: int, timeout: float) -> None: - assert self._client is not None - start = time.monotonic() - time.sleep(0.05) - while time.monotonic() - start < timeout: - state = self._client.get_gripper_state() - pos = self._client.get_position_per_mille() - self._status.gripper_state = state - self._status.finger_position_per_mille = float(pos) - self._status.position = self._per_mille_to_mm(pos) - self._status.is_moving = state == 0 - self._status.is_gripped = state == 2 - if state in (1, 2): - self._status.state = GripperState.GRIPPING if state == 2 else GripperState.IDLE - return - if state == 3: - self._status.state = GripperState.ERROR - self._status.error_msg = "夹持后物体掉落" - return - if abs(pos - target_per_mille) <= self.cfg.position_tolerance_per_mille and state != 0: - self._status.state = GripperState.IDLE - return - time.sleep(self.cfg.status_poll_interval) - self._status.state = GripperState.ERROR - self._status.error_msg = f"平行夹爪运动超时（>{timeout:.1f}s）" - - def get_status(self) -> GripperStatus: - if not self._connected or self._client is None: - self._status.connected = False - return self._status - try: - pos = self._client.get_position_per_mille() - state = self._client.get_gripper_state() - init_state = self._client.get_init_state() - self._status.connected = True - self._status.initialized = init_state == 1 - self._status.finger_position_per_mille = float(pos) - self._status.position = self._per_mille_to_mm(pos) - self._status.velocity = float(self._last_speed) - self._status.force = float(self._last_force) - self._status.gripper_state = state - self._status.is_moving = state == 0 - self._status.is_gripped = state == 2 - if self._status.state != GripperState.ERROR: - self._status.state = GripperState.MOVING if state == 0 else ( - GripperState.GRIPPING if state == 2 else GripperState.IDLE - ) - self._status.timestamp = time.time() - except Exception as exc: - self._status.state = GripperState.ERROR - self._status.error_msg = str(exc) - self._status.connected = False - return self._status - - def open(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus: - # DH 千分比：0‰=闭合，1000‰=张开，对应行程 0mm -> stroke_max。 - return self.execute(GripperCommand( - target_position=float(self.cfg.stroke_max), - target_force=force or self.cfg.default_force, - target_velocity=speed or self.cfg.default_speed, - **kwargs, - )) - - def close(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus: - return self.execute(GripperCommand( - target_position=float(self.cfg.stroke_min), - target_force=force or self.cfg.default_force, - target_velocity=speed or self.cfg.default_speed, - **kwargs, - )) - - def move_to(self, position_mm: float, force: int | None = None, speed: int | None = None, - **kwargs) -> GripperStatus: - return self.execute(GripperCommand( - target_position=position_mm, - target_force=force or self.cfg.default_force, - target_velocity=speed or self.cfg.default_speed, - **kwargs, - )) - - def set_force(self, force_pct: int) -> None: - self._require_client() - assert self._client is not None - self._last_force = self._client.set_force_pct(force_pct) - - def set_speed(self, speed_pct: int) -> None: - self._require_client() - assert self._client is not None - self._last_speed = self._client.set_speed_pct(speed_pct) - - def stop(self) -> None: - if self._client is not None: - self._client.hold_current_position() - self._status.state = GripperState.IDLE - - -# ============================================================================= -# 大寰旋转夹爪（RGI-100-30-O-S-M1） -# ============================================================================= - -@dataclass -class RotaryGripperConfig: - """大寰 RGI 旋转夹爪配置。""" - - port: str = "/dev/ttyUSB1" - slave_id: int = 1 - baudrate: int = 115200 - timeout: float = 0.2 - - # RGI 旋转角度寄存器是 signed 16bit；为避免 one's-complement 负零歧义，默认限制在 ±32767。 - angle_min: float = -32767.0 - angle_max: float = 32767.0 - max_step_deg: float = 30000.0 # rotate_revolutions 分段旋转的最大单步角度 - - # DH 手册：手指位置 0~1000‰。0=闭合，1000=张开。RGI-100 行程约 70mm。 - finger_open_per_mille: int = 1000 - finger_close_per_mille: int = 0 - - # 默认参数（DH 百分比） - default_speed: int = 50 # 旋转速度 % - default_torque: int = 50 # 旋转力/力矩 % - default_finger_speed: int = 50 # 手指速度 % - default_finger_force: int = 50 # 手指夹持力 % - torque_min: int = 20 - torque_max: int = 100 - - # 控制行为 - auto_initialize: bool = True - init_timeout: float = 15.0 # RGI 初始化包含旋转找零，时间更长 - angle_tolerance_deg: float = 1.0 - status_poll_interval: float = 0.05 - - -class RotaryGripper(BaseGripper): - """大寰 RGI-100-30-O-S-M1 旋转夹爪。""" - - name = "dh_rgi_rotary" - gripper_type = GripperType.ROTARY - - def __init__(self, config: RotaryGripperConfig | None = None) -> None: - super().__init__() - self.cfg = config or RotaryGripperConfig() - self._client: DHModbusClient | None = None - self._last_angle = 0.0 - self._last_torque = int(self.cfg.default_torque) - self._last_speed = int(self.cfg.default_speed) - self._last_finger_force = int(self.cfg.default_finger_force) - self._last_finger_speed = int(self.cfg.default_finger_speed) - self._last_finger_per_mille = int(self.cfg.finger_open_per_mille) - - def connect(self) -> None: - if self._client is not None: - self.disconnect() - self._client = DHModbusClient( - self.cfg.port, - slave_id=self.cfg.slave_id, - baudrate=self.cfg.baudrate, - timeout=self.cfg.timeout, - ) - self._connected = True - self._status.connected = True - self._status.state = GripperState.IDLE - self._status.error_msg = "" - if self.cfg.auto_initialize: - self.initialize() - - def disconnect(self) -> None: - if self._client is not None: - self._client.close() - self._client = None - self._connected = False - self._status.connected = False - self._status.state = GripperState.IDLE - - def initialize(self, full: bool = False) -> None: - self._require_client() - assert self._client is not None - self._client.initialize(full=full, rotation=True, timeout=self.cfg.init_timeout) - self._status.initialized = True - - def _require_client(self) -> None: - if not self._connected or self._client is None: - self._status.state = GripperState.ERROR - self._status.error_msg = "旋转夹爪未连接" - raise DHModbusError(self._status.error_msg) - - def execute(self, cmd: GripperCommand) -> GripperStatus: - """执行旋转指令；cmd.target_position 单位为度。""" - return self.rotate_to( - self._last_angle if cmd.target_position is None else float(cmd.target_position), - speed=cmd.target_velocity, - torque=cmd.target_force, - wait_complete=cmd.wait_complete, - timeout=cmd.timeout, - ) - - def _wait_complete(self, target_angle: float, timeout: float) -> None: - assert self._client is not None - start = time.monotonic() - start_angle = float(self._client.get_rotation_angle_deg()) - expected = abs(float(target_angle) - start_angle) + 30.0 - settled = 0 - last_angle = start_angle - time.sleep(0.05) - while time.monotonic() - start < timeout: - rot_state = self._client.get_rotation_state() - angle = float(self._client.get_rotation_angle_deg()) - self._status.rotation_state = rot_state - self._status.rotation_deg = angle - self._status.position = angle - self._status.is_moving = rot_state == 0 - if abs(angle - start_angle) > expected: - self._client.hold_current_rotation() - self._status.state = GripperState.ERROR - self._status.error_msg = ( - f"旋转超程保护：目标 {target_angle:.1f}°，从 {start_angle:.1f}° 跑到 {angle:.1f}°，已发停止" - ) - return - if rot_state in (1, 2): - self._status.state = GripperState.IDLE if rot_state == 1 else GripperState.GRIPPING - if rot_state == 2: - self._status.error_msg = "旋转受阻（blocking）" - return - if rot_state == 3: - self._status.state = GripperState.ERROR - self._status.error_msg = "旋转曾受阻且未到目标位" - return - if abs(angle - float(target_angle)) <= self.cfg.angle_tolerance_deg: - settled += 1 - if settled >= 3: - self._status.state = GripperState.IDLE - self._status.is_moving = False - return - elif abs(angle - last_angle) <= self.cfg.angle_tolerance_deg: - settled += 1 - if settled >= 8 and abs(angle - float(target_angle)) <= 15.0: - self._status.state = GripperState.IDLE - self._status.is_moving = False - return - else: - settled = 0 - last_angle = angle - time.sleep(self.cfg.status_poll_interval) - self._client.hold_current_rotation() - self._status.state = GripperState.ERROR - self._status.error_msg = f"旋转运动超时（>{timeout:.1f}s），已发停止" - - def get_status(self) -> GripperStatus: - if not self._connected or self._client is None: - self._status.connected = False - return self._status - try: - finger_pos = self._client.get_position_per_mille() - finger_state = self._client.get_gripper_state() - init_state = self._client.get_init_state() - rot_angle = self._client.get_rotation_angle_deg() - rot_state = self._client.get_rotation_state() - rot_init = self._client.get_rotation_init_state() - - self._status.connected = True - self._status.initialized = init_state == 1 and rot_init == 1 - self._status.finger_position_per_mille = float(finger_pos) - self._status.rotation_deg = float(rot_angle) - self._status.position = float(rot_angle) - self._status.velocity = float(self._last_speed) - self._status.force = float(self._last_torque) - self._status.gripper_state = finger_state - self._status.rotation_state = rot_state - self._status.is_moving = finger_state == 0 or rot_state == 0 - self._status.is_gripped = finger_state == 2 - if self._status.state != GripperState.ERROR: - if finger_state == 2 or rot_state == 2: - self._status.state = GripperState.GRIPPING - elif finger_state == 0 or rot_state == 0: - self._status.state = GripperState.MOVING - else: - self._status.state = GripperState.IDLE - self._status.timestamp = time.time() - except Exception as exc: - self._status.state = GripperState.ERROR - self._status.error_msg = str(exc) - self._status.connected = False - return self._status - - # ----------------------------- - # 手指夹持 - # ----------------------------- - def move_fingers(self, position_per_mille: int, force: int | None = None, - speed: int | None = None, wait_complete: bool = True, - timeout: float = 10.0) -> GripperStatus: - try: - self._require_client() - except DHModbusError: - return self._status - assert self._client is not None - target = int(np.clip(position_per_mille, 0, 1000)) - f = int(np.clip(force or self.cfg.default_finger_force, 20, 100)) - s = int(np.clip(speed or self.cfg.default_finger_speed, 1, 100)) - self._client.set_speed_pct(s) - self._client.set_force_pct(f) - self._client.set_position_per_mille(target) - self._last_finger_per_mille = target - self._last_finger_force = f - self._last_finger_speed = s - if wait_complete: - start = time.monotonic() - time.sleep(0.05) - while time.monotonic() - start < timeout: - state = self._client.get_gripper_state() - pos = self._client.get_position_per_mille() - self._status.gripper_state = state - self._status.finger_position_per_mille = float(pos) - if state in (1, 2): - break - if state == 3: - self._status.state = GripperState.ERROR - self._status.error_msg = "夹持后物体掉落" - break - time.sleep(self.cfg.status_poll_interval) - return self.get_status() - - def open_fingers(self, **kwargs) -> GripperStatus: - return self.move_fingers(self.cfg.finger_open_per_mille, **kwargs) - - def close_fingers(self, **kwargs) -> GripperStatus: - return self.move_fingers(self.cfg.finger_close_per_mille, **kwargs) - - # ----------------------------- - # 旋转 - # ----------------------------- - def rotate_to(self, angle_deg: float, speed: int | None = None, torque: int | None = None, - wait_complete: bool = True, timeout: float = 10.0, **kwargs) -> GripperStatus: - try: - self._require_client() - except DHModbusError: - return self._status - assert self._client is not None - - angle = float(np.clip(angle_deg, self.cfg.angle_min, self.cfg.angle_max)) - s = int(np.clip(speed or self.cfg.default_speed, 1, 100)) - t = int(np.clip(torque or self.cfg.default_torque, self.cfg.torque_min, self.cfg.torque_max)) - - self._client.set_rotation_speed_pct(s) - self._client.set_rotation_force_pct(t) - target = self._client.set_rotation_angle_deg(angle) - - self._last_angle = float(target) - self._last_speed = s - self._last_torque = t - self._status.state = GripperState.MOVING - self._status.error_msg = "" - - if wait_complete: - self._wait_complete(float(target), timeout) - return self.get_status() - - def rotate_by(self, delta_deg: float, **kwargs) -> GripperStatus: - current = self.get_status().rotation_deg - return self.rotate_to(current + float(delta_deg), **kwargs) - - def rotate_revolutions(self, revolutions: float, **kwargs) -> GripperStatus: - """旋转指定圈数；超过单步角度范围时自动分段，并按实际反馈角度结算。""" - remaining = float(revolutions) * 360.0 - status = self.get_status() - max_step = abs(float(self.cfg.max_step_deg)) or 30000.0 - while abs(remaining) > 1e-6: - before = float(self.get_status().rotation_deg) - step = float(np.clip(remaining, -max_step, max_step)) - status = self.rotate_to(before + step, **kwargs) - if status.state == GripperState.ERROR: - return status - after = float(status.rotation_deg) - achieved = after - before - if abs(achieved) < max(1.0, abs(step) * 0.2): - status.state = GripperState.ERROR - status.error_msg = ( - "旋转未达到预期步进，可能超出角度寄存器范围或反馈异常：" - f"step={step:.3f}, before={before:.3f}, after={after:.3f}" - ) - return status - remaining -= achieved - return status - - def home_rotation(self, **kwargs) -> GripperStatus: - return self.rotate_to(0.0, **kwargs) - - def set_rotation_speed(self, speed_pct: int) -> None: - self._require_client() - assert self._client is not None - self._last_speed = self._client.set_rotation_speed_pct(speed_pct) - - def set_rotation_torque(self, torque_pct: int) -> None: - self._require_client() - assert self._client is not None - self._last_torque = self._client.set_rotation_force_pct(torque_pct) - - def set_finger_speed(self, speed_pct: int) -> None: - self._require_client() - assert self._client is not None - self._last_finger_speed = self._client.set_speed_pct(speed_pct) - - def set_finger_force(self, force_pct: int) -> None: - self._require_client() - assert self._client is not None - self._last_finger_force = self._client.set_force_pct(force_pct) - - def open(self, **kwargs) -> GripperStatus: - """对于 RGI，open 定义为张开手指；旋转请用 rotate_to/rotate_revolutions。""" - return self.open_fingers(**kwargs) - - def close(self, **kwargs) -> GripperStatus: - """对于 RGI，close 定义为闭合手指；旋转请用 rotate_to/rotate_revolutions。""" - return self.close_fingers(**kwargs) - - def stop(self) -> None: - if self._client is not None: - self._client.hold_current_all() - self._status.state = GripperState.IDLE - - -# ============================================================================= -# 双臂夹爪管理器 -# ============================================================================= - -@dataclass -class DualArmGripperConfig: - """双臂夹爪配置。""" - - left_arm_sn: str = "Rizon4-063989" - right_arm_sn: str = "Rizon4-063991" - left_gripper: ParallelGripperConfig = field(default_factory=ParallelGripperConfig) - right_gripper: RotaryGripperConfig = field(default_factory=RotaryGripperConfig) - - -class DualArmGripperManager: - """双臂夹爪管理器：统一控制左右臂夹爪。""" - - def __init__(self, config: DualArmGripperConfig | None = None) -> None: - self.cfg = config or DualArmGripperConfig() - self._left: ParallelGripper | None = None - self._right: RotaryGripper | None = None - - @property - def left(self) -> ParallelGripper: - if self._left is None: - self._left = ParallelGripper(self.cfg.left_gripper) - return self._left - - @property - def right(self) -> RotaryGripper: - if self._right is None: - self._right = RotaryGripper(self.cfg.right_gripper) - return self._right - - def connect_all(self) -> None: - self.left.connect() - self.right.connect() - - def disconnect_all(self) -> None: - if self._left: - self._left.disconnect() - if self._right: - self._right.disconnect() - - def initialize_all(self, full: bool = False) -> None: - self.left.initialize(full=full) - self.right.initialize(full=full) - - def get_all_status(self) -> dict[str, GripperStatus]: - return { - "left": self.left.get_status(), - "right": self.right.get_status(), - } - - def emergency_stop(self) -> None: - self.left.stop() - self.right.stop() - - -def load_dual_arm_gripper_config(path: str | Path) -> DualArmGripperConfig: - """从 configs/dual_arm_robot.yaml 加载双臂夹爪配置。""" - import yaml - - data = yaml.safe_load(Path(path).read_text(encoding="utf-8")) or {} - left = data.get("left_arm", {}) or {} - right = data.get("right_arm", {}) or {} - grip = data.get("gripper", {}) or {} - parallel = grip.get("parallel", {}) or {} - rotary = grip.get("rotary", {}) or {} - - left_cfg = ParallelGripperConfig( - port=left.get("gripper_port", "/dev/ttyUSB0"), - slave_id=int(left.get("gripper_slave_id", 1)), - baudrate=int(left.get("gripper_baudrate", 115200)), - stroke_min=float(parallel.get("stroke_min", 0.0)), - stroke_max=float(parallel.get("stroke_max", 40.0)), - default_speed=int(parallel.get("default_speed", 50)), - default_force=int(parallel.get("default_force", 50)), - force_min=int(parallel.get("force_min", 20)), - force_max=int(parallel.get("force_max", 100)), - auto_initialize=bool(parallel.get("auto_initialize", True)), - ) - right_cfg = RotaryGripperConfig( - port=right.get("gripper_port", "/dev/ttyUSB1"), - slave_id=int(right.get("gripper_slave_id", 1)), - baudrate=int(right.get("gripper_baudrate", 115200)), - angle_min=float(rotary.get("angle_min", -32767.0)), - angle_max=float(rotary.get("angle_max", 32767.0)), - max_step_deg=float(rotary.get("max_step_deg", 30000.0)), - finger_open_per_mille=int(rotary.get("finger_open_per_mille", 1000)), - finger_close_per_mille=int(rotary.get("finger_close_per_mille", 0)), - default_speed=int(rotary.get("default_speed", 50)), - default_torque=int(rotary.get("default_torque", 50)), - default_finger_speed=int(rotary.get("default_finger_speed", 50)), - default_finger_force=int(rotary.get("default_finger_force", 50)), - torque_min=int(rotary.get("torque_min", 20)), - torque_max=int(rotary.get("torque_max", 100)), - auto_initialize=bool(rotary.get("auto_initialize", True)), - ) - return DualArmGripperConfig( - left_arm_sn=left.get("robot_sn", "Rizon4-063989"), - right_arm_sn=right.get("robot_sn", "Rizon4-063991"), - left_gripper=left_cfg, - right_gripper=right_cfg, - ) - - -def load_from_site(data: dict) -> DualArmGripperConfig: - """从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。""" - robots = list((data.get("robot") or {}).get("robots") or []) - left = next((r for r in robots if str(r.get("arm") or "").lower() == "left"), {}) - right = next((r for r in robots if str(r.get("arm") or "").lower() == "right"), {}) - if not left: - left = next((r for r in robots if "063989" in str(r.get("sn") or "")), {}) - if not right: - right = next((r for r in robots if "063991" in str(r.get("sn") or "")), {}) - g = data.get("gripper") or {} - parallel = g.get("parallel") or {} - rotary = g.get("rotary") or {} - lg = left.get("gripper") or {} - rg = right.get("gripper") or {} - stroke = float(lg.get("stroke_mm") or parallel.get("stroke_max") or g.get("stroke_mm") or 20.0) - left_cfg = ParallelGripperConfig( - port=str(lg.get("port") or "/dev/ttyUSB0"), - slave_id=int(lg.get("slave_id") or g.get("slave_id") or 1), - baudrate=int(lg.get("baud") or g.get("baud") or 115200), - stroke_min=float(parallel.get("stroke_min") or 0.0), - stroke_max=stroke, - default_speed=int(parallel.get("default_speed") or g.get("speed_pct") or 50), - default_force=int(parallel.get("default_force") or g.get("force_pct") or 50), - force_min=int(parallel.get("force_min") or 20), - force_max=int(parallel.get("force_max") or 100), - auto_initialize=bool(lg.get("auto_init", parallel.get("auto_initialize", True))), - init_timeout=float(parallel.get("init_timeout") or 10.0), - ) - right_cfg = RotaryGripperConfig( - port=str(rg.get("port") or "/dev/ttyUSB1"), - slave_id=int(rg.get("slave_id") or 1), - baudrate=int(rg.get("baud") or g.get("baud") or 115200), - angle_min=float(rotary.get("angle_min") or -32767.0), - angle_max=float(rotary.get("angle_max") or 32767.0), - max_step_deg=float(rotary.get("max_step_deg") or 30000.0), - finger_open_per_mille=int(rotary.get("finger_open_per_mille") or 1000), - finger_close_per_mille=int(rotary.get("finger_close_per_mille") or 0), - default_speed=int(rotary.get("default_speed") or 50), - default_torque=int(rotary.get("default_torque") or 50), - default_finger_speed=int(rotary.get("default_finger_speed") or 50), - default_finger_force=int(rotary.get("default_finger_force") or 50), - torque_min=int(rotary.get("torque_min") or 20), - torque_max=int(rotary.get("torque_max") or 100), - auto_initialize=bool(rg.get("auto_init", rotary.get("auto_initialize", False))), - init_timeout=float(rotary.get("init_timeout") or 15.0), - ) - return DualArmGripperConfig( - left_arm_sn=str(left.get("sn") or "Rizon4-063989"), - right_arm_sn=str(right.get("sn") or "Rizon4-063991"), - left_gripper=left_cfg, - right_gripper=right_cfg, - ) +"""双臂夹爪统一控制架构（DH-Robotics 官方 Modbus RTU，非 Flexiv RDK）。 + +设计目标： +- 左臂（Rizon4-063989）：大寰 PGEA 平行夹爪，控制行程/夹持力/速度。 +- 右臂（Rizon4-063991）：大寰 RGI-100-30-O-S-M1 旋转夹爪，控制手指夹持与旋转角度/圈数/速度。 +- 统一接口，便于后续扩展到 ROS2 节点和新项目脚本。 + +注意：DH 官方协议是“写位置/角度寄存器后立即运动”，没有 ChangingTek SDK 的 0x0108 触发寄存器。 +""" + +from __future__ import annotations + +import time +from abc import ABC, abstractmethod +from dataclasses import dataclass, field +from enum import Enum, auto +from pathlib import Path +from typing import Any + +import numpy as np + +from arm_application.dh_official import DHModbusClient, DHModbusError + + +class GripperType(Enum): + PARALLEL = auto() # 平行夹爪 + ROTARY = auto() # 旋转夹爪 + VACUUM = auto() # 真空吸盘（预留） + + +class GripperState(Enum): + IDLE = auto() + MOVING = auto() + GRIPPING = auto() + RELEASING = auto() + ERROR = auto() + + +@dataclass +class GripperStatus: + """夹爪状态反馈。""" + + state: GripperState = GripperState.IDLE + position: float = 0.0 # 平行：mm；旋转：角度 deg + force: float = 0.0 # 最近一次命令力/力矩 % + velocity: float = 0.0 # 最近一次命令速度 % + is_moving: bool = False + is_gripped: bool = False # 是否夹持到物体 + connected: bool = False + initialized: bool = False + finger_position_per_mille: float = 0.0 + rotation_deg: float = 0.0 + gripper_state: int = -1 # DH 0x0201：0 moving,1 reached,2 caught,3 dropped + rotation_state: int = -1 # RGI 0x020B：0 moving,1 reached,2 blocking,3 blocked + error_msg: str = "" + timestamp: float = field(default_factory=time.time) + + +@dataclass +class GripperCommand: + """夹爪控制指令。""" + + target_position: float | None = None # 平行：mm；旋转：deg + target_force: float | None = None # 目标力/力矩 % + target_velocity: float | None = None # 目标速度 % + wait_complete: bool = True # 是否等待完成 + timeout: float = 10.0 # 超时时间 + + +class BaseGripper(ABC): + """夹爪基类，定义统一接口。""" + + name: str = "base_gripper" + gripper_type: GripperType = GripperType.PARALLEL + + def __init__(self, config: dict[str, Any] | None = None) -> None: + self.cfg = config or {} + self._connected = False + self._status = GripperStatus() + + @abstractmethod + def connect(self) -> None: ... + + @abstractmethod + def disconnect(self) -> None: ... + + @abstractmethod + def execute(self, cmd: GripperCommand) -> GripperStatus: ... + + @abstractmethod + def get_status(self) -> GripperStatus: ... + + def is_connected(self) -> bool: + return self._connected + + def open(self, **kwargs) -> GripperStatus: + raise NotImplementedError + + def close(self, **kwargs) -> GripperStatus: + raise NotImplementedError + + def stop(self) -> None: + raise NotImplementedError + + +# ============================================================================= +# 大寰平行夹爪（PGE/PGEA 系列） +# ============================================================================= + +@dataclass +class ParallelGripperConfig: + """大寰平行夹爪配置。""" + + port: str = "/dev/ttyUSB0" + slave_id: int = 1 + baudrate: int = 115200 + timeout: float = 0.2 + + # PGEA-100-40 行程为 40mm；若实际是 PGEA-100-26，请把 stroke_max 改成 26.0。 + stroke_min: float = 0.0 + stroke_max: float = 40.0 + + # 默认参数（DH 百分比） + default_speed: int = 50 + default_force: int = 50 + force_min: int = 20 + force_max: int = 100 + + # 控制行为 + auto_initialize: bool = True + init_timeout: float = 10.0 + position_tolerance_per_mille: int = 15 + status_poll_interval: float = 0.05 + + +class ParallelGripper(BaseGripper): + """大寰 PGE/PGEA 平行夹爪。""" + + name = "dh_pgea_parallel" + gripper_type = GripperType.PARALLEL + + def __init__(self, config: ParallelGripperConfig | None = None) -> None: + super().__init__() + self.cfg = config or ParallelGripperConfig() + self._client: DHModbusClient | None = None + self._last_position_mm = float(self.cfg.stroke_min) + self._last_force = int(self.cfg.default_force) + self._last_speed = int(self.cfg.default_speed) + + def connect(self) -> None: + if self._client is not None: + self.disconnect() + self._client = DHModbusClient( + self.cfg.port, + slave_id=self.cfg.slave_id, + baudrate=self.cfg.baudrate, + timeout=self.cfg.timeout, + ) + self._connected = True + self._status.connected = True + self._status.state = GripperState.IDLE + self._status.error_msg = "" + if self.cfg.auto_initialize: + self.initialize() + + def disconnect(self) -> None: + if self._client is not None: + self._client.close() + self._client = None + self._connected = False + self._status.connected = False + self._status.state = GripperState.IDLE + + def initialize(self, full: bool = False) -> None: + self._require_client() + assert self._client is not None + self._client.initialize(full=full, rotation=False, timeout=self.cfg.init_timeout) + self._status.initialized = True + + def _require_client(self) -> None: + if not self._connected or self._client is None: + self._status.state = GripperState.ERROR + self._status.error_msg = "平行夹爪未连接" + raise DHModbusError(self._status.error_msg) + + def _mm_to_per_mille(self, position_mm: float) -> int: + lo = float(self.cfg.stroke_min) + hi = float(self.cfg.stroke_max) + if hi <= lo: + hi = lo + 1.0 + ratio = (float(position_mm) - lo) / (hi - lo) + return int(np.clip(round(ratio * 1000.0), 0, 1000)) + + def _per_mille_to_mm(self, position: float | int) -> float: + lo = float(self.cfg.stroke_min) + hi = float(self.cfg.stroke_max) + return lo + (float(position) / 1000.0) * (hi - lo) + + def execute(self, cmd: GripperCommand) -> GripperStatus: + try: + self._require_client() + except DHModbusError: + return self._status + assert self._client is not None + + position_mm = self._last_position_mm if cmd.target_position is None else float(cmd.target_position) + position_mm = float(np.clip(position_mm, self.cfg.stroke_min, self.cfg.stroke_max)) + speed = int(np.clip(cmd.target_velocity or self.cfg.default_speed, 1, 100)) + force = int(np.clip(cmd.target_force or self.cfg.default_force, self.cfg.force_min, self.cfg.force_max)) + + self._client.set_speed_pct(speed) + self._client.set_force_pct(force) + target_per_mille = self._client.set_position_per_mille(self._mm_to_per_mille(position_mm)) + + self._last_position_mm = position_mm + self._last_speed = speed + self._last_force = force + self._status.state = GripperState.MOVING + self._status.error_msg = "" + + if cmd.wait_complete: + self._wait_complete(target_per_mille, cmd.timeout) + return self.get_status() + + def _wait_complete(self, target_per_mille: int, timeout: float) -> None: + assert self._client is not None + start = time.monotonic() + time.sleep(0.05) + while time.monotonic() - start < timeout: + state = self._client.get_gripper_state() + pos = self._client.get_position_per_mille() + self._status.gripper_state = state + self._status.finger_position_per_mille = float(pos) + self._status.position = self._per_mille_to_mm(pos) + self._status.is_moving = state == 0 + self._status.is_gripped = state == 2 + if state in (1, 2): + self._status.state = GripperState.GRIPPING if state == 2 else GripperState.IDLE + return + if state == 3: + self._status.state = GripperState.ERROR + self._status.error_msg = "夹持后物体掉落" + return + if abs(pos - target_per_mille) <= self.cfg.position_tolerance_per_mille and state != 0: + self._status.state = GripperState.IDLE + return + time.sleep(self.cfg.status_poll_interval) + self._status.state = GripperState.ERROR + self._status.error_msg = f"平行夹爪运动超时（>{timeout:.1f}s）" + + def get_status(self) -> GripperStatus: + if not self._connected or self._client is None: + self._status.connected = False + return self._status + try: + pos = self._client.get_position_per_mille() + state = self._client.get_gripper_state() + init_state = self._client.get_init_state() + self._status.connected = True + self._status.initialized = init_state == 1 + self._status.finger_position_per_mille = float(pos) + self._status.position = self._per_mille_to_mm(pos) + self._status.velocity = float(self._last_speed) + self._status.force = float(self._last_force) + self._status.gripper_state = state + self._status.is_moving = state == 0 + self._status.is_gripped = state == 2 + if self._status.state != GripperState.ERROR: + self._status.state = GripperState.MOVING if state == 0 else ( + GripperState.GRIPPING if state == 2 else GripperState.IDLE + ) + self._status.timestamp = time.time() + except Exception as exc: + self._status.state = GripperState.ERROR + self._status.error_msg = str(exc) + self._status.connected = False + return self._status + + def open(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus: + # DH 千分比：0‰=闭合，1000‰=张开，对应行程 0mm -> stroke_max。 + return self.execute(GripperCommand( + target_position=float(self.cfg.stroke_max), + target_force=force or self.cfg.default_force, + target_velocity=speed or self.cfg.default_speed, + **kwargs, + )) + + def close(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus: + return self.execute(GripperCommand( + target_position=float(self.cfg.stroke_min), + target_force=force or self.cfg.default_force, + target_velocity=speed or self.cfg.default_speed, + **kwargs, + )) + + def move_to(self, position_mm: float, force: int | None = None, speed: int | None = None, + **kwargs) -> GripperStatus: + return self.execute(GripperCommand( + target_position=position_mm, + target_force=force or self.cfg.default_force, + target_velocity=speed or self.cfg.default_speed, + **kwargs, + )) + + def set_force(self, force_pct: int) -> None: + self._require_client() + assert self._client is not None + self._last_force = self._client.set_force_pct(force_pct) + + def set_speed(self, speed_pct: int) -> None: + self._require_client() + assert self._client is not None + self._last_speed = self._client.set_speed_pct(speed_pct) + + def stop(self) -> None: + if self._client is not None: + self._client.hold_current_position() + self._status.state = GripperState.IDLE + + +# ============================================================================= +# 大寰旋转夹爪（RGI-100-30-O-S-M1） +# ============================================================================= + +@dataclass +class RotaryGripperConfig: + """大寰 RGI 旋转夹爪配置。""" + + port: str = "/dev/ttyUSB1" + slave_id: int = 1 + baudrate: int = 115200 + timeout: float = 0.2 + + # RGI 旋转角度寄存器是 signed 16bit；为避免 one's-complement 负零歧义，默认限制在 ±32767。 + angle_min: float = -32767.0 + angle_max: float = 32767.0 + max_step_deg: float = 30000.0 # rotate_revolutions 分段旋转的最大单步角度 + + # DH 手册：手指位置 0~1000‰。0=闭合，1000=张开。RGI-100 行程约 70mm。 + finger_open_per_mille: int = 1000 + finger_close_per_mille: int = 0 + + # 默认参数（DH 百分比） + default_speed: int = 50 # 旋转速度 % + default_torque: int = 50 # 旋转力/力矩 % + default_finger_speed: int = 50 # 手指速度 % + default_finger_force: int = 50 # 手指夹持力 % + torque_min: int = 20 + torque_max: int = 100 + + # 控制行为 + auto_initialize: bool = True + init_timeout: float = 15.0 # RGI 初始化包含旋转找零，时间更长 + angle_tolerance_deg: float = 1.0 + status_poll_interval: float = 0.05 + + +class RotaryGripper(BaseGripper): + """大寰 RGI-100-30-O-S-M1 旋转夹爪。""" + + name = "dh_rgi_rotary" + gripper_type = GripperType.ROTARY + + def __init__(self, config: RotaryGripperConfig | None = None) -> None: + super().__init__() + self.cfg = config or RotaryGripperConfig() + self._client: DHModbusClient | None = None + self._last_angle = 0.0 + self._last_torque = int(self.cfg.default_torque) + self._last_speed = int(self.cfg.default_speed) + self._last_finger_force = int(self.cfg.default_finger_force) + self._last_finger_speed = int(self.cfg.default_finger_speed) + self._last_finger_per_mille = int(self.cfg.finger_open_per_mille) + + def connect(self) -> None: + if self._client is not None: + self.disconnect() + self._client = DHModbusClient( + self.cfg.port, + slave_id=self.cfg.slave_id, + baudrate=self.cfg.baudrate, + timeout=self.cfg.timeout, + ) + self._connected = True + self._status.connected = True + self._status.state = GripperState.IDLE + self._status.error_msg = "" + if self.cfg.auto_initialize: + self.initialize() + + def disconnect(self) -> None: + if self._client is not None: + self._client.close() + self._client = None + self._connected = False + self._status.connected = False + self._status.state = GripperState.IDLE + + def initialize(self, full: bool = False) -> None: + self._require_client() + assert self._client is not None + self._client.initialize(full=full, rotation=True, timeout=self.cfg.init_timeout) + self._status.initialized = True + + def _require_client(self) -> None: + if not self._connected or self._client is None: + self._status.state = GripperState.ERROR + self._status.error_msg = "旋转夹爪未连接" + raise DHModbusError(self._status.error_msg) + + def execute(self, cmd: GripperCommand) -> GripperStatus: + """执行旋转指令；cmd.target_position 单位为度。""" + return self.rotate_to( + self._last_angle if cmd.target_position is None else float(cmd.target_position), + speed=cmd.target_velocity, + torque=cmd.target_force, + wait_complete=cmd.wait_complete, + timeout=cmd.timeout, + ) + + def _wait_complete(self, target_angle: float, timeout: float) -> None: + assert self._client is not None + start = time.monotonic() + start_angle = float(self._client.get_rotation_angle_deg()) + expected = abs(float(target_angle) - start_angle) + 30.0 + settled = 0 + last_angle = start_angle + time.sleep(0.05) + while time.monotonic() - start < timeout: + rot_state = self._client.get_rotation_state() + angle = float(self._client.get_rotation_angle_deg()) + self._status.rotation_state = rot_state + self._status.rotation_deg = angle + self._status.position = angle + self._status.is_moving = rot_state == 0 + if abs(angle - start_angle) > expected: + self._client.hold_current_rotation() + self._status.state = GripperState.ERROR + self._status.error_msg = ( + f"旋转超程保护：目标 {target_angle:.1f}°，从 {start_angle:.1f}° 跑到 {angle:.1f}°，已发停止" + ) + return + if rot_state in (1, 2): + self._status.state = GripperState.IDLE if rot_state == 1 else GripperState.GRIPPING + if rot_state == 2: + self._status.error_msg = "旋转受阻（blocking）" + return + if rot_state == 3: + self._status.state = GripperState.ERROR + self._status.error_msg = "旋转曾受阻且未到目标位" + return + if abs(angle - float(target_angle)) <= self.cfg.angle_tolerance_deg: + settled += 1 + if settled >= 3: + self._status.state = GripperState.IDLE + self._status.is_moving = False + return + elif abs(angle - last_angle) <= self.cfg.angle_tolerance_deg: + settled += 1 + if settled >= 8 and abs(angle - float(target_angle)) <= 15.0: + self._status.state = GripperState.IDLE + self._status.is_moving = False + return + else: + settled = 0 + last_angle = angle + time.sleep(self.cfg.status_poll_interval) + self._client.hold_current_rotation() + self._status.state = GripperState.ERROR + self._status.error_msg = f"旋转运动超时（>{timeout:.1f}s），已发停止" + + def get_status(self) -> GripperStatus: + if not self._connected or self._client is None: + self._status.connected = False + return self._status + try: + finger_pos = self._client.get_position_per_mille() + finger_state = self._client.get_gripper_state() + init_state = self._client.get_init_state() + rot_angle = self._client.get_rotation_angle_deg() + rot_state = self._client.get_rotation_state() + rot_init = self._client.get_rotation_init_state() + + self._status.connected = True + self._status.initialized = init_state == 1 and rot_init == 1 + self._status.finger_position_per_mille = float(finger_pos) + self._status.rotation_deg = float(rot_angle) + self._status.position = float(rot_angle) + self._status.velocity = float(self._last_speed) + self._status.force = float(self._last_torque) + self._status.gripper_state = finger_state + self._status.rotation_state = rot_state + self._status.is_moving = finger_state == 0 or rot_state == 0 + self._status.is_gripped = finger_state == 2 + if self._status.state != GripperState.ERROR: + if finger_state == 2 or rot_state == 2: + self._status.state = GripperState.GRIPPING + elif finger_state == 0 or rot_state == 0: + self._status.state = GripperState.MOVING + else: + self._status.state = GripperState.IDLE + self._status.timestamp = time.time() + except Exception as exc: + self._status.state = GripperState.ERROR + self._status.error_msg = str(exc) + self._status.connected = False + return self._status + + # ----------------------------- + # 手指夹持 + # ----------------------------- + def move_fingers(self, position_per_mille: int, force: int | None = None, + speed: int | None = None, wait_complete: bool = True, + timeout: float = 10.0) -> GripperStatus: + try: + self._require_client() + except DHModbusError: + return self._status + assert self._client is not None + target = int(np.clip(position_per_mille, 0, 1000)) + f = int(np.clip(force or self.cfg.default_finger_force, 20, 100)) + s = int(np.clip(speed or self.cfg.default_finger_speed, 1, 100)) + self._client.set_speed_pct(s) + self._client.set_force_pct(f) + self._client.set_position_per_mille(target) + self._last_finger_per_mille = target + self._last_finger_force = f + self._last_finger_speed = s + if wait_complete: + start = time.monotonic() + time.sleep(0.05) + while time.monotonic() - start < timeout: + state = self._client.get_gripper_state() + pos = self._client.get_position_per_mille() + self._status.gripper_state = state + self._status.finger_position_per_mille = float(pos) + if state in (1, 2): + break + if state == 3: + self._status.state = GripperState.ERROR + self._status.error_msg = "夹持后物体掉落" + break + time.sleep(self.cfg.status_poll_interval) + return self.get_status() + + def open_fingers(self, **kwargs) -> GripperStatus: + return self.move_fingers(self.cfg.finger_open_per_mille, **kwargs) + + def close_fingers(self, **kwargs) -> GripperStatus: + return self.move_fingers(self.cfg.finger_close_per_mille, **kwargs) + + # ----------------------------- + # 旋转 + # ----------------------------- + def rotate_to(self, angle_deg: float, speed: int | None = None, torque: int | None = None, + wait_complete: bool = True, timeout: float = 10.0, **kwargs) -> GripperStatus: + try: + self._require_client() + except DHModbusError: + return self._status + assert self._client is not None + + angle = float(np.clip(angle_deg, self.cfg.angle_min, self.cfg.angle_max)) + s = int(np.clip(speed or self.cfg.default_speed, 1, 100)) + t = int(np.clip(torque or self.cfg.default_torque, self.cfg.torque_min, self.cfg.torque_max)) + + self._client.set_rotation_speed_pct(s) + self._client.set_rotation_force_pct(t) + target = self._client.set_rotation_angle_deg(angle) + + self._last_angle = float(target) + self._last_speed = s + self._last_torque = t + self._status.state = GripperState.MOVING + self._status.error_msg = "" + + if wait_complete: + self._wait_complete(float(target), timeout) + return self.get_status() + + def rotate_by(self, delta_deg: float, **kwargs) -> GripperStatus: + current = self.get_status().rotation_deg + return self.rotate_to(current + float(delta_deg), **kwargs) + + def rotate_revolutions(self, revolutions: float, **kwargs) -> GripperStatus: + """旋转指定圈数；超过单步角度范围时自动分段，并按实际反馈角度结算。""" + remaining = float(revolutions) * 360.0 + status = self.get_status() + max_step = abs(float(self.cfg.max_step_deg)) or 30000.0 + while abs(remaining) > 1e-6: + before = float(self.get_status().rotation_deg) + step = float(np.clip(remaining, -max_step, max_step)) + status = self.rotate_to(before + step, **kwargs) + if status.state == GripperState.ERROR: + return status + after = float(status.rotation_deg) + achieved = after - before + if abs(achieved) < max(1.0, abs(step) * 0.2): + status.state = GripperState.ERROR + status.error_msg = ( + "旋转未达到预期步进，可能超出角度寄存器范围或反馈异常：" + f"step={step:.3f}, before={before:.3f}, after={after:.3f}" + ) + return status + remaining -= achieved + return status + + def home_rotation(self, **kwargs) -> GripperStatus: + return self.rotate_to(0.0, **kwargs) + + def set_rotation_speed(self, speed_pct: int) -> None: + self._require_client() + assert self._client is not None + self._last_speed = self._client.set_rotation_speed_pct(speed_pct) + + def set_rotation_torque(self, torque_pct: int) -> None: + self._require_client() + assert self._client is not None + self._last_torque = self._client.set_rotation_force_pct(torque_pct) + + def set_finger_speed(self, speed_pct: int) -> None: + self._require_client() + assert self._client is not None + self._last_finger_speed = self._client.set_speed_pct(speed_pct) + + def set_finger_force(self, force_pct: int) -> None: + self._require_client() + assert self._client is not None + self._last_finger_force = self._client.set_force_pct(force_pct) + + def open(self, **kwargs) -> GripperStatus: + """对于 RGI，open 定义为张开手指；旋转请用 rotate_to/rotate_revolutions。""" + return self.open_fingers(**kwargs) + + def close(self, **kwargs) -> GripperStatus: + """对于 RGI，close 定义为闭合手指；旋转请用 rotate_to/rotate_revolutions。""" + return self.close_fingers(**kwargs) + + def stop(self) -> None: + if self._client is not None: + self._client.hold_current_all() + self._status.state = GripperState.IDLE + + +# ============================================================================= +# 双臂夹爪管理器 +# ============================================================================= + +@dataclass +class DualArmGripperConfig: + """双臂夹爪配置。""" + + left_arm_sn: str = "Rizon4-063989" + right_arm_sn: str = "Rizon4-063991" + left_gripper: ParallelGripperConfig = field(default_factory=ParallelGripperConfig) + right_gripper: RotaryGripperConfig = field(default_factory=RotaryGripperConfig) + + +class DualArmGripperManager: + """双臂夹爪管理器：统一控制左右臂夹爪。""" + + def __init__(self, config: DualArmGripperConfig | None = None) -> None: + self.cfg = config or DualArmGripperConfig() + self._left: ParallelGripper | None = None + self._right: RotaryGripper | None = None + + @property + def left(self) -> ParallelGripper: + if self._left is None: + self._left = ParallelGripper(self.cfg.left_gripper) + return self._left + + @property + def right(self) -> RotaryGripper: + if self._right is None: + self._right = RotaryGripper(self.cfg.right_gripper) + return self._right + + def connect_all(self) -> None: + self.left.connect() + self.right.connect() + + def disconnect_all(self) -> None: + if self._left: + self._left.disconnect() + if self._right: + self._right.disconnect() + + def initialize_all(self, full: bool = False) -> None: + self.left.initialize(full=full) + self.right.initialize(full=full) + + def get_all_status(self) -> dict[str, GripperStatus]: + return { + "left": self.left.get_status(), + "right": self.right.get_status(), + } + + def emergency_stop(self) -> None: + self.left.stop() + self.right.stop() + + +def load_dual_arm_gripper_config(path: str | Path) -> DualArmGripperConfig: + """从 configs/dual_arm_robot.yaml 加载双臂夹爪配置。""" + import yaml + + data = yaml.safe_load(Path(path).read_text(encoding="utf-8")) or {} + left = data.get("left_arm", {}) or {} + right = data.get("right_arm", {}) or {} + grip = data.get("gripper", {}) or {} + parallel = grip.get("parallel", {}) or {} + rotary = grip.get("rotary", {}) or {} + + left_cfg = ParallelGripperConfig( + port=left.get("gripper_port", "/dev/ttyUSB0"), + slave_id=int(left.get("gripper_slave_id", 1)), + baudrate=int(left.get("gripper_baudrate", 115200)), + stroke_min=float(parallel.get("stroke_min", 0.0)), + stroke_max=float(parallel.get("stroke_max", 40.0)), + default_speed=int(parallel.get("default_speed", 50)), + default_force=int(parallel.get("default_force", 50)), + force_min=int(parallel.get("force_min", 20)), + force_max=int(parallel.get("force_max", 100)), + auto_initialize=bool(parallel.get("auto_initialize", True)), + ) + right_cfg = RotaryGripperConfig( + port=right.get("gripper_port", "/dev/ttyUSB1"), + slave_id=int(right.get("gripper_slave_id", 1)), + baudrate=int(right.get("gripper_baudrate", 115200)), + angle_min=float(rotary.get("angle_min", -32767.0)), + angle_max=float(rotary.get("angle_max", 32767.0)), + max_step_deg=float(rotary.get("max_step_deg", 30000.0)), + finger_open_per_mille=int(rotary.get("finger_open_per_mille", 1000)), + finger_close_per_mille=int(rotary.get("finger_close_per_mille", 0)), + default_speed=int(rotary.get("default_speed", 50)), + default_torque=int(rotary.get("default_torque", 50)), + default_finger_speed=int(rotary.get("default_finger_speed", 50)), + default_finger_force=int(rotary.get("default_finger_force", 50)), + torque_min=int(rotary.get("torque_min", 20)), + torque_max=int(rotary.get("torque_max", 100)), + auto_initialize=bool(rotary.get("auto_initialize", True)), + ) + return DualArmGripperConfig( + left_arm_sn=left.get("robot_sn", "Rizon4-063989"), + right_arm_sn=right.get("robot_sn", "Rizon4-063991"), + left_gripper=left_cfg, + right_gripper=right_cfg, + ) + + +def load_from_site(data: dict) -> DualArmGripperConfig: + """从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。""" + robots = list((data.get("robot") or {}).get("robots") or []) + left = next((r for r in robots if str(r.get("arm") or "").lower() == "left"), {}) + right = next((r for r in robots if str(r.get("arm") or "").lower() == "right"), {}) + if not left: + left = next((r for r in robots if "063989" in str(r.get("sn") or "")), {}) + if not right: + right = next((r for r in robots if "063991" in str(r.get("sn") or "")), {}) + g = data.get("gripper") or {} + parallel = g.get("parallel") or {} + rotary = g.get("rotary") or {} + lg = left.get("gripper") or {} + rg = right.get("gripper") or {} + stroke = float(lg.get("stroke_mm") or parallel.get("stroke_max") or g.get("stroke_mm") or 20.0) + left_cfg = ParallelGripperConfig( + port=str(lg.get("port") or "/dev/ttyUSB0"), + slave_id=int(lg.get("slave_id") or g.get("slave_id") or 1), + baudrate=int(lg.get("baud") or g.get("baud") or 115200), + stroke_min=float(parallel.get("stroke_min") or 0.0), + stroke_max=stroke, + default_speed=int(parallel.get("default_speed") or g.get("speed_pct") or 50), + default_force=int(parallel.get("default_force") or g.get("force_pct") or 50), + force_min=int(parallel.get("force_min") or 20), + force_max=int(parallel.get("force_max") or 100), + auto_initialize=bool(lg.get("auto_init", parallel.get("auto_initialize", True))), + init_timeout=float(parallel.get("init_timeout") or 10.0), + ) + right_cfg = RotaryGripperConfig( + port=str(rg.get("port") or "/dev/ttyUSB1"), + slave_id=int(rg.get("slave_id") or 1), + baudrate=int(rg.get("baud") or g.get("baud") or 115200), + angle_min=float(rotary.get("angle_min") or -32767.0), + angle_max=float(rotary.get("angle_max") or 32767.0), + max_step_deg=float(rotary.get("max_step_deg") or 30000.0), + finger_open_per_mille=int(rotary.get("finger_open_per_mille") or 1000), + finger_close_per_mille=int(rotary.get("finger_close_per_mille") or 0), + default_speed=int(rotary.get("default_speed") or 50), + default_torque=int(rotary.get("default_torque") or 50), + default_finger_speed=int(rotary.get("default_finger_speed") or 50), + default_finger_force=int(rotary.get("default_finger_force") or 50), + torque_min=int(rotary.get("torque_min") or 20), + torque_max=int(rotary.get("torque_max") or 100), + auto_initialize=bool(rg.get("auto_init", rotary.get("auto_initialize", False))), + init_timeout=float(rotary.get("init_timeout") or 15.0), + ) + return DualArmGripperConfig( + left_arm_sn=str(left.get("sn") or "Rizon4-063989"), + right_arm_sn=str(right.get("sn") or "Rizon4-063991"), + left_gripper=left_cfg, + right_gripper=right_cfg, + ) diff --git a/src/arm_application/arm_application/dual_gripper_cli.py b/src/arm_application/arm_application/dual_gripper_cli.py index 7269f6d..1041fc5 100644 --- a/src/arm_application/arm_application/dual_gripper_cli.py +++ b/src/arm_application/arm_application/dual_gripper_cli.py @@ -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()) diff --git a/src/arm_application/arm_application/dual_gripper_node.py b/src/arm_application/arm_application/dual_gripper_node.py index 17c4987..729d34c 100644 --- a/src/arm_application/arm_application/dual_gripper_node.py +++ b/src/arm_application/arm_application/dual_gripper_node.py @@ -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() diff --git a/src/arm_application/arm_application/hole_detect_node.py b/src/arm_application/arm_application/hole_detect_node.py index ae547fa..c34f14c 100644 --- a/src/arm_application/arm_application/hole_detect_node.py +++ b/src/arm_application/arm_application/hole_detect_node.py @@ -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() diff --git a/src/arm_application/arm_application/hole_detector.py b/src/arm_application/arm_application/hole_detector.py index 87bdc57..148b0e4 100644 --- a/src/arm_application/arm_application/hole_detector.py +++ b/src/arm_application/arm_application/hole_detector.py @@ -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) diff --git a/src/arm_application/arm_application/site_io.py b/src/arm_application/arm_application/site_io.py index 10a8a73..05f2326 100644 --- a/src/arm_application/arm_application/site_io.py +++ b/src/arm_application/arm_application/site_io.py @@ -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 diff --git a/src/arm_application/arm_application/tactile_cli.py b/src/arm_application/arm_application/tactile_cli.py index d8cced6..f9dc5ad 100644 --- a/src/arm_application/arm_application/tactile_cli.py +++ b/src/arm_application/arm_application/tactile_cli.py @@ -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()) diff --git a/src/arm_application/arm_application/tactile_interface.py b/src/arm_application/arm_application/tactile_interface.py index 6f369ea..6c121ee 100644 --- a/src/arm_application/arm_application/tactile_interface.py +++ b/src/arm_application/arm_application/tactile_interface.py @@ -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}", + ) diff --git a/src/arm_application/config/dual_grippers.yaml b/src/arm_application/config/dual_grippers.yaml index 0132c6f..1bcd231 100644 --- a/src/arm_application/config/dual_grippers.yaml +++ b/src/arm_application/config/dual_grippers.yaml @@ -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 diff --git a/src/arm_application/config/hole_insert.yaml b/src/arm_application/config/hole_insert.yaml index 37ed698..6ebe007 100644 --- a/src/arm_application/config/hole_insert.yaml +++ b/src/arm_application/config/hole_insert.yaml @@ -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 diff --git a/src/arm_application/launch/dual_gripper.launch.py b/src/arm_application/launch/dual_gripper.launch.py index 59571f4..13831cf 100644 --- a/src/arm_application/launch/dual_gripper.launch.py +++ b/src/arm_application/launch/dual_gripper.launch.py @@ -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, + } + ], + ), + ] + ) diff --git a/src/arm_application/launch/hole_detect.launch.py b/src/arm_application/launch/hole_detect.launch.py index 8a72a78..08c9970 100644 --- a/src/arm_application/launch/hole_detect.launch.py +++ b/src/arm_application/launch/hole_detect.launch.py @@ -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), + ] + ) diff --git a/src/arm_msgs/msg/GripperCommand.msg b/src/arm_msgs/msg/GripperCommand.msg index 24312c4..e8a3701 100644 --- a/src/arm_msgs/msg/GripperCommand.msg +++ b/src/arm_msgs/msg/GripperCommand.msg @@ -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 diff --git a/src/arm_msgs/msg/GripperStatus.msg b/src/arm_msgs/msg/GripperStatus.msg index 2e5909b..581ac43 100644 --- a/src/arm_msgs/msg/GripperStatus.msg +++ b/src/arm_msgs/msg/GripperStatus.msg @@ -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 diff --git a/src/arm_msgs/msg/HoleDetection.msg b/src/arm_msgs/msg/HoleDetection.msg index 1bcb543..1922ee4 100644 --- a/src/arm_msgs/msg/HoleDetection.msg +++ b/src/arm_msgs/msg/HoleDetection.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 diff --git a/src/arm_msgs/msg/HoleDetections.msg b/src/arm_msgs/msg/HoleDetections.msg index 83e2111..75cb798 100644 --- a/src/arm_msgs/msg/HoleDetections.msg +++ b/src/arm_msgs/msg/HoleDetections.msg @@ -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 diff --git a/src/arm_msgs/msg/TactileStatus.msg b/src/arm_msgs/msg/TactileStatus.msg index a10897f..207b383 100644 --- a/src/arm_msgs/msg/TactileStatus.msg +++ b/src/arm_msgs/msg/TactileStatus.msg @@ -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 diff --git a/src/arm_msgs/srv/GripperExecute.srv b/src/arm_msgs/srv/GripperExecute.srv index 2c0070c..9061133 100644 --- a/src/arm_msgs/srv/GripperExecute.srv +++ b/src/arm_msgs/srv/GripperExecute.srv @@ -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