From 83b32542ef3a7ae48756c9a143a63e3677476740 Mon Sep 17 00:00:00 2001 From: FallenSigh Date: Tue, 16 Jun 2026 18:45:01 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=B7=BB=E5=8A=A0=E6=9C=BA=E6=A2=B0?= =?UTF-8?q?=E8=87=82=20ROS=202=20=E6=8E=A7=E5=88=B6=E8=8A=82=E7=82=B9?= =?UTF-8?q?=E5=92=8C=E8=A7=86=E8=A7=89=E6=8A=93=E5=8F=96=E7=B3=BB=E7=BB=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 创建 arm_control_msgs 包：定义机械臂控制的消息和服务接口 - 消息：JointState, TCPPose - 服务：MoveJoints, MovePose, GetPose, SetGripper - 实现 arm_control 节点：独立的机械臂控制 ROS 节点 - 完整的逆运动学和正运动学 - 关节空间和笛卡尔空间运动控制 - UDP 通信与 ESP32 - 状态发布（10Hz） - 实现 vision_grasp 节点：自动化视觉抓取 - 相机坐标系到基坐标系的完整变换 - 自动抓取流程：释放→移动→抓取→回收 - 自动释放流程：移动→释放→回收 - 多线程执行器支持 - 添加完整文档 - ARM_CONTROL_README.md: 机械臂控制节点使用指南 - VISION_GRASP_README.md: 视觉抓取节点使用指南 - QUICKSTART.md: 快速开始指南 - 文档重命名：docs/craic.md → docs/arm.md --- docs/ARM_CONTROL_README.md | 319 ++++++++ docs/BUILD_SUCCESS.md | 122 +++ docs/IMPLEMENTATION_SUMMARY.md | 252 ++++++ docs/QUICKSTART.md | 244 ++++++ docs/VISION_GRASP_README.md | 359 +++++++++ docs/VISION_GRASP_SUMMARY.md | 286 +++++++ docs/{craic.md => arm.md} | 0 ros2/src/arm_control_msgs/CMakeLists.txt | 25 + ros2/src/arm_control_msgs/msg/JointState.msg | 8 + ros2/src/arm_control_msgs/msg/TCPPose.msg | 6 + ros2/src/arm_control_msgs/package.xml | 22 + ros2/src/arm_control_msgs/srv/GetPose.srv | 14 + ros2/src/arm_control_msgs/srv/MoveJoints.srv | 11 + ros2/src/arm_control_msgs/srv/MovePose.srv | 18 + ros2/src/arm_control_msgs/srv/SetGripper.srv | 12 + ros2/src/udp_teleop/config/arm_control.yaml | 38 + ros2/src/udp_teleop/config/vision_grasp.yaml | 21 + ros2/src/udp_teleop/package.xml | 8 +- ros2/src/udp_teleop/setup.py | 4 +- ros2/src/udp_teleop/udp_teleop/arm_control.py | 743 ++++++++++++++++++ .../udp_teleop/arm_control_client.py | 182 +++++ .../src/udp_teleop/udp_teleop/vision_grasp.py | 428 ++++++++++ 22 files changed, 3119 insertions(+), 3 deletions(-) create mode 100644 docs/ARM_CONTROL_README.md create mode 100644 docs/BUILD_SUCCESS.md create mode 100644 docs/IMPLEMENTATION_SUMMARY.md create mode 100644 docs/QUICKSTART.md create mode 100644 docs/VISION_GRASP_README.md create mode 100644 docs/VISION_GRASP_SUMMARY.md rename docs/{craic.md => arm.md} (100%) create mode 100644 ros2/src/arm_control_msgs/CMakeLists.txt create mode 100644 ros2/src/arm_control_msgs/msg/JointState.msg create mode 100644 ros2/src/arm_control_msgs/msg/TCPPose.msg create mode 100644 ros2/src/arm_control_msgs/package.xml create mode 100644 ros2/src/arm_control_msgs/srv/GetPose.srv create mode 100644 ros2/src/arm_control_msgs/srv/MoveJoints.srv create mode 100644 ros2/src/arm_control_msgs/srv/MovePose.srv create mode 100644 ros2/src/arm_control_msgs/srv/SetGripper.srv create mode 100644 ros2/src/udp_teleop/config/arm_control.yaml create mode 100644 ros2/src/udp_teleop/config/vision_grasp.yaml create mode 100644 ros2/src/udp_teleop/udp_teleop/arm_control.py create mode 100644 ros2/src/udp_teleop/udp_teleop/arm_control_client.py create mode 100644 ros2/src/udp_teleop/udp_teleop/vision_grasp.py diff --git a/docs/ARM_CONTROL_README.md b/docs/ARM_CONTROL_README.md new file mode 100644 index 0000000..38a1906 --- /dev/null +++ b/docs/ARM_CONTROL_README.md @@ -0,0 +1,319 @@ +# arm_control ROS 节点使用指南 + +## 概述 + +`arm_control` 是一个封装了机械臂控制功能的 ROS 2 节点，基于 `udp_control.py` 改造，提供服务接口进行机械臂控制。 + +## 功能特性 + +- ✅ 关节空间运动控制（带插值） +- ✅ 笛卡尔空间运动控制（带逆运动学） +- ✅ 正运动学查询 +- ✅ 夹爪控制 +- ✅ 状态发布（关节状态 + TCP 位姿） +- ✅ 状态缓存（平滑运动） + +## 编译 + +```bash +cd ros2 + +# 1. 编译消息包 +colcon build --packages-select arm_control_msgs + +# 2. Source 消息包 +source install/setup.bash + +# 3. 编译控制节点 +colcon build --packages-select udp_teleop + +# 4. Source 控制节点 +source install/setup.bash +``` + +## 运行 + +### 启动控制节点 + +```bash +# 使用默认参数 +ros2 run udp_teleop arm_control + +# 使用配置文件 +ros2 run udp_teleop arm_control \ + --ros-args --params-file src/udp_teleop/config/arm_control.yaml + +# 覆盖特定参数 +ros2 run udp_teleop arm_control \ + --ros-args -p udp_ip:=192.168.233.67 -p udp_port:=8888 +``` + +## 服务接口 + +### 1. 关节空间运动 + +```bash +ros2 service call /arm_control/move_joints arm_control_msgs/srv/MoveJoints \ + "{height: -100, j2: 10, j3: 20, j4: 30, j5: 81, j6: 30, duration: 2.0}" +``` + +### 2. 笛卡尔空间运动 + +```bash +# 基本运动 +ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ + "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}" + +# 带夹爪控制 +ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ + "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, grip: true, duration: 2.0}" +``` + +### 3. 查询当前位姿 + +```bash +ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose +``` + +输出示例： +``` +success: true +message: '' +x: 150.234 +y: 75.123 +z: -100.0 +phi: 45.678 +height: -100 +j2: 13 +j3: 27 +j4: 55 +j5: 81 +j6: 30 +``` + +### 4. 夹爪控制 + +```bash +# 抓取 +ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \ + "{grip: true}" + +# 释放 +ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \ + "{release: true}" +``` + +## 话题订阅 + +### 1. 关节状态 + +```bash +ros2 topic echo /arm_control/joint_states +``` + +输出： +```yaml +header: + stamp: + sec: 1234567890 + nanosec: 123456789 + frame_id: '' +height: -100 +j2: 13 +j3: 27 +j4: 55 +j5: 81 +j6: 30 +``` + +### 2. TCP 位姿 + +```bash +ros2 topic echo /arm_control/tcp_pose +``` + +输出： +```yaml +header: + stamp: + sec: 1234567890 + nanosec: 123456789 + frame_id: '' +x: 150.234 +y: 75.123 +z: -100.0 +phi: 45.678 +``` + +## Python 客户端示例 + +```python +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from arm_control_msgs.srv import MovePose + +class MyArmController(Node): + def __init__(self): + super().__init__('my_controller') + self.cli = self.create_client(MovePose, 'arm_control/move_pose') + self.cli.wait_for_service() + + def move_to(self, x, y, z, phi): + req = MovePose.Request() + req.x = x + req.y = y + req.z = z + req.phi = phi + req.duration = 2.0 + + future = self.cli.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result().success + +def main(): + rclpy.init() + controller = MyArmController() + + # 移动到目标位置 + controller.move_to(200.0, 100.0, -100.0, 45.0) + + controller.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() +``` + +## 完整抓取流程示例 + +```bash +# 运行示例客户端（包含完整抓取流程） +ros2 run udp_teleop arm_control_client +``` + +或手动调用： + +```bash +# 1. 查询当前位姿 +ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose + +# 2. 移动到物体上方 +ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ + "{x: 200.0, y: 100.0, z: -50.0, phi: 45.0, release: true, duration: 2.0}" + +# 3. 下降到抓取位置 +ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ + "{x: 200.0, y: 100.0, z: -150.0, phi: 45.0, release: true, duration: 1.0}" + +# 4. 抓取 +ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \ + "{grip: true}" + +# 5. 提升 +ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ + "{x: 200.0, y: 100.0, z: -50.0, phi: 45.0, grip: true, duration: 1.0}" +``` + +## 参数配置 + +编辑 `config/arm_control.yaml`： + +```yaml +arm_control: + ros__parameters: + # UDP 配置 + udp_ip: '192.168.4.1' + udp_port: 8888 + + # 机械臂几何参数 + l1: 125.0 + l2: 125.0 + x4: 110.0 + z4: 80.0 + + # 关节限位 + height_min: -290 + height_max: 0 + j2_min: -110 + j2_max: 115 + # ... (更多参数见配置文件) +``` + +## 调试 + +### 查看服务列表 + +```bash +ros2 service list | grep arm_control +``` + +### 查看话题列表 + +```bash +ros2 topic list | grep arm_control +``` + +### 查看服务接口定义 + +```bash +ros2 interface show arm_control_msgs/srv/MovePose +``` + +### 实时监控状态 + +```bash +# 终端 1: 查看关节状态 +ros2 topic echo /arm_control/joint_states + +# 终端 2: 查看 TCP 位姿 +ros2 topic echo /arm_control/tcp_pose + +# 终端 3: 发送控制命令 +ros2 service call /arm_control/move_pose ... +``` + +## 常见问题 + +### Q1: 服务调用失败 + +**检查**： +1. 节点是否正在运行？`ros2 node list` +2. UDP 连接是否正常？检查 `udp_ip` 参数 +3. 关节限位是否合理？查看错误消息 + +### Q2: 运动不平滑 + +**调整参数**： +- 增加 `duration`（运动时长） +- 增加 `default_rate`（插值频率） + +### Q3: 状态不更新 + +**检查**： +- `use_state_cache` 是否启用？ +- `tools/.udp_control_state.json` 是否可写？ + +## 与原始 udp_control.py 对比 + +| 功能 | udp_control.py | arm_control 节点 | +|------|---------------|-----------------| +| 接口 | 命令行 | ROS 服务 + 话题 | +| 集成 | 独立脚本 | ROS 生态系统 | +| 状态查询 | 文件缓存 | 服务调用 | +| 多客户端 | 不支持 | 支持 | +| 实时监控 | 不支持 | 话题订阅 | + +## 下一步 + +- 集成视觉系统：创建视觉抓取节点，订阅相机话题，调用 arm_control 服务 +- 添加轨迹规划：创建轨迹规划器，生成平滑路径 +- 碰撞检测：添加工作空间限制和碰撞检测 + +## 相关文件 + +- 节点实现：`udp_teleop/arm_control.py` +- 消息定义：`arm_control_msgs/msg/` +- 服务定义：`arm_control_msgs/srv/` +- 配置文件：`udp_teleop/config/arm_control.yaml` +- 示例客户端：`udp_teleop/arm_control_client.py` diff --git a/docs/BUILD_SUCCESS.md b/docs/BUILD_SUCCESS.md new file mode 100644 index 0000000..ecacc20 --- /dev/null +++ b/docs/BUILD_SUCCESS.md @@ -0,0 +1,122 @@ +# 编译成功！🎉 + +## ✅ 已完成 + +1. **消息包编译** - arm_control_msgs ✓ +2. **控制节点编译** - udp_teleop ✓ + +## 🚀 快速测试 + +### 1. 启动控制节点 + +```bash +# 激活环境 +conda activate ros2_humble + +# Source 工作空间 +cd ros2 +source install/setup.bash + +# 启动节点（修改 IP 为你的 ESP32 IP） +ros2 run udp_teleop arm_control \ + --ros-args --params-file src/udp_teleop/config/arm_control.yaml +``` + +### 2. 测试服务（新终端） + +```bash +# 激活环境 +conda activate ros2_humble +cd ros2 +source install/setup.bash + +# 查询当前位姿 +ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose + +# 移动到指定位置 +ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ + "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}" +``` + +### 3. 查看状态 + +```bash +# 查看关节状态 +ros2 topic echo /arm_control/joint_states + +# 查看 TCP 位姿 +ros2 topic echo /arm_control/tcp_pose + +# 查看所有服务 +ros2 service list | grep arm_control +``` + +## ⚠️ 重要提示 + +### 编译说明 + +由于 robostack 的 Python 配置问题，编译时需要显式指定 Python 路径： + +```bash +# 已在 build_arm_control.sh 中自动处理 +export PYTHON_EXECUTABLE=$CONDA_PREFIX/bin/python +export PYTHON_INCLUDE_DIR=$CONDA_PREFIX/include/python3.12 +export PYTHON_LIBRARY=$CONDA_PREFIX/lib/libpython3.12.so +``` + +### 修改配置 + +编辑 `src/udp_teleop/config/arm_control.yaml` 修改参数： + +```yaml +arm_control: + ros__parameters: + udp_ip: '192.168.4.1' # 修改为你的 ESP32 IP + udp_port: 8888 +``` + +修改后直接重启节点即可，无需重新编译。 + +## 📝 下一步 + +1. **修改 ESP32 IP**: 编辑 `config/arm_control.yaml` +2. **测试连接**: 启动节点，查看是否有错误 +3. **调用服务**: 使用上面的命令测试 +4. **运行示例**: `ros2 run udp_teleop arm_control_client` + +## 🐛 故障排查 + +### 问题：找不到服务 + +**解决**： +```bash +# 检查节点是否运行 +ros2 node list + +# 重新 source 环境 +source install/setup.bash +``` + +### 问题：UDP 发送失败 + +**解决**： +1. 检查 ESP32 IP 是否正确 +2. 测试网络连接：`ping 192.168.4.1` +3. 测试 UDP：`echo 'XYW:0:0:0:XZHY' | nc -u 192.168.4.1 8888` + +### 问题：重新编译 + +**解决**： +```bash +# 清理后重新编译 +rm -rf build/ install/ log/ +./build_arm_control.sh +``` + +## 📚 文档 + +- 完整文档：[ARM_CONTROL_README.md](ARM_CONTROL_README.md) +- 快速指南：[QUICKSTART.md](QUICKSTART.md) +- 实现总结：[IMPLEMENTATION_SUMMARY.md](IMPLEMENTATION_SUMMARY.md) + +祝使用愉快！🎉 diff --git a/docs/IMPLEMENTATION_SUMMARY.md b/docs/IMPLEMENTATION_SUMMARY.md new file mode 100644 index 0000000..f43ae00 --- /dev/null +++ b/docs/IMPLEMENTATION_SUMMARY.md @@ -0,0 +1,252 @@ +# arm_control ROS 节点封装总结 + +## ✅ 完成的工作 + +### 1. 创建了消息和服务定义包 (`arm_control_msgs`) + +**消息类型**： +- `TCPPose.msg` - TCP 位姿（x, y, z, phi） +- `JointState.msg` - 关节状态（height, j2-j6） + +**服务类型**： +- `MoveJoints.srv` - 关节空间运动控制 +- `MovePose.srv` - 笛卡尔空间运动控制（带逆运动学） +- `GetPose.srv` - 查询当前位姿（正运动学） +- `SetGripper.srv` - 夹爪控制 + +### 2. 封装了控制节点 (`arm_control.py`) + +**核心功能**： +- ✅ 关节空间插值运动 +- ✅ 笛卡尔空间逆运动学求解 +- ✅ 正运动学位姿计算 +- ✅ UDP 命令发送（与 ESP32 通信） +- ✅ 状态缓存（平滑运动） +- ✅ 参数化配置 +- ✅ 状态发布（10Hz） + +**服务接口**： +- `/arm_control/move_joints` - 关节运动 +- `/arm_control/move_pose` - 位姿运动 +- `/arm_control/get_pose` - 查询位姿 +- `/arm_control/set_gripper` - 夹爪控制 + +**话题发布**： +- `/arm_control/joint_states` - 关节状态（10Hz） +- `/arm_control/tcp_pose` - TCP 位姿（10Hz） + +### 3. 创建了示例客户端 (`arm_control_client.py`) + +**演示功能**： +- 查询当前位姿 +- 完整抓取流程： + 1. 移动到物体上方 + 2. 下降 + 3. 抓取 + 4. 提升 + 5. 移动到目标位置 + 6. 下降 + 7. 释放 + 8. 提升 + +### 4. 配置和文档 + +**配置文件**： +- `config/arm_control.yaml` - 完整参数配置 + +**文档**： +- `ARM_CONTROL_README.md` - 完整使用文档 +- `QUICKSTART.md` - 快速开始指南 + +**脚本**： +- `build_arm_control.sh` - 一键编译脚本 + +## 📁 文件清单 + +``` +ros2/ +├── build_arm_control.sh # 编译脚本 ✨ +├── ARM_CONTROL_README.md # 完整文档 ✨ +├── QUICKSTART.md # 快速指南 ✨ +└── src/ + ├── arm_control_msgs/ # 消息包 ✨ + │ ├── CMakeLists.txt + │ ├── package.xml + │ ├── msg/ + │ │ ├── TCPPose.msg + │ │ └── JointState.msg + │ └── srv/ + │ ├── MoveJoints.srv + │ ├── MovePose.srv + │ ├── GetPose.srv + │ └── SetGripper.srv + └── udp_teleop/ + ├── setup.py # 已更新 ✨ + ├── package.xml # 已更新 ✨ + ├── udp_teleop/ + │ ├── keyboard_control.py # 原有 + │ ├── arm_control.py # 新增 ✨ + │ └── arm_control_client.py # 新增 ✨ + └── config/ + ├── params.yaml # 原有 + └── arm_control.yaml # 新增 ✨ +``` + +## 🚀 快速使用 + +### 编译 + +```bash +cd ros2 +./build_arm_control.sh +``` + +### 运行节点 + +```bash +ros2 run udp_teleop arm_control \ + --ros-args --params-file src/udp_teleop/config/arm_control.yaml +``` + +### 测试服务 + +```bash +# 查询位姿 +ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose + +# 移动 +ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ + "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}" +``` + +### 运行示例 + +```bash +ros2 run udp_teleop arm_control_client +``` + +## 🎯 与原始 udp_control.py 对比 + +| 特性 | udp_control.py | arm_control 节点 | +|------|---------------|-----------------| +| **接口方式** | 命令行参数 | ROS 服务调用 | +| **状态查询** | 读取 JSON 文件 | 服务调用 + 话题订阅 | +| **多客户端** | ❌ 不支持 | ✅ 支持 | +| **实时监控** | ❌ 无 | ✅ 10Hz 状态发布 | +| **参数配置** | 命令行参数 | YAML 配置文件 | +| **集成度** | 独立工具 | ROS 生态集成 | +| **可编程性** | Shell 脚本 | Python/C++ 客户端 | + +## 💡 优势 + +### 1. **标准化接口** +- 使用 ROS 服务和话题，符合 ROS 生态标准 +- 易于与其他 ROS 节点集成（如视觉、规划器） + +### 2. **多客户端支持** +- 多个客户端可同时连接 +- 适合复杂系统（如视觉 + 手动控制） + +### 3. **实时状态监控** +- 10Hz 状态发布 +- 可用于可视化、日志记录、故障诊断 + +### 4. **灵活配置** +- YAML 参数文件 +- 运行时参数覆盖 +- 无需重新编译 + +### 5. **易于扩展** +- 添加新服务：只需定义 .srv 文件 +- 添加新话题：只需定义 .msg 文件 +- 集成其他功能：订阅/发布话题即可 + +## 🔧 使用场景 + +### 场景 1：视觉抓取 + +```python +# 视觉节点订阅相机话题，检测物体 +# 调用 arm_control 服务控制机械臂 +class VisionGraspNode(Node): + def __init__(self): + self.arm_cli = self.create_client(MovePose, 'arm_control/move_pose') + self.sub = self.create_subscription(Image, '/camera/image', self.on_image, 10) + + def on_image(self, msg): + # 检测物体 + x, y, z = detect_object(msg) + + # 控制机械臂抓取 + self.move_to(x, y, z, phi=45.0) +``` + +### 场景 2：示教编程 + +```python +# 记录示教点位 +class TeachPendant(Node): + def __init__(self): + self.get_cli = self.create_client(GetPose, 'arm_control/get_pose') + self.move_cli = self.create_client(MovePose, 'arm_control/move_pose') + self.waypoints = [] + + def record_waypoint(self): + # 记录当前位置 + pose = self.get_current_pose() + self.waypoints.append(pose) + + def replay(self): + # 重放示教轨迹 + for pose in self.waypoints: + self.move_to(pose.x, pose.y, pose.z, pose.phi) +``` + +### 场景 3：轨迹规划 + +```python +# 使用规划器生成轨迹 +class TrajectoryPlanner(Node): + def __init__(self): + self.move_cli = self.create_client(MovePose, 'arm_control/move_pose') + + def execute_trajectory(self, waypoints): + # 执行轨迹点序列 + for wp in waypoints: + self.move_to(wp.x, wp.y, wp.z, wp.phi, duration=0.5) +``` + +## 📚 下一步建议 + +### 1. **视觉集成** +创建视觉抓取节点，结合 `camera_to_base.py` 实现自动抓取 + +### 2. **GUI 控制面板** +使用 RQt 创建图形界面，实时显示状态和控制 + +### 3. **轨迹记录与回放** +实现示教编程功能 + +### 4. **碰撞检测** +添加工作空间限制和简单碰撞检测 + +### 5. **MoveIt 集成** +创建 URDF 和 MoveIt 配置，使用高级运动规划 + +## 🎓 学习资源 + +- ROS 2 服务教程：https://docs.ros.org/en/humble/Tutorials/Services.html +- ROS 2 话题教程：https://docs.ros.org/en/humble/Tutorials/Topics.html +- 自定义消息：https://docs.ros.org/en/humble/Tutorials/Custom-ROS2-Interfaces.html + +## ✨ 总结 + +现在你有了一个完整的 ROS 节点化的机械臂控制系统： + +1. ✅ **功能完整** - 保留了 udp_control.py 的所有功能 +2. ✅ **接口标准** - 使用 ROS 服务和话题 +3. ✅ **易于集成** - 可与其他 ROS 节点无缝配合 +4. ✅ **文档齐全** - 提供了完整的文档和示例 +5. ✅ **开箱即用** - 一键编译，快速上手 + +祝你使用愉快！🎉 diff --git a/docs/QUICKSTART.md b/docs/QUICKSTART.md new file mode 100644 index 0000000..96afcfc --- /dev/null +++ b/docs/QUICKSTART.md @@ -0,0 +1,244 @@ +# 机械臂控制 ROS 节点 - 快速开始 + +## 🚀 快速编译和运行 + +### 1. 一键编译 + +```bash +cd ros2 +./build_arm_control.sh +``` + +### 2. 启动节点 + +```bash +# 方法 A: 使用配置文件（推荐） +ros2 run udp_teleop arm_control \ + --ros-args --params-file src/udp_teleop/config/arm_control.yaml + +# 方法 B: 使用默认参数 +ros2 run udp_teleop arm_control + +# 方法 C: 覆盖特定参数 +ros2 run udp_teleop arm_control \ + --ros-args -p udp_ip:=192.168.233.67 +``` + +### 3. 测试服务 + +```bash +# 查询当前位姿 +ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose + +# 移动到指定位置 +ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ + "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}" +``` + +### 4. 运行完整示例 + +```bash +# 在新终端运行示例客户端（包含完整抓取流程） +ros2 run udp_teleop arm_control_client +``` + +## 📁 文件结构 + +``` +ros2/ +├── build_arm_control.sh # 一键编译脚本 +├── ARM_CONTROL_README.md # 完整使用文档 +├── QUICKSTART.md # 本文件 +└── src/ + ├── arm_control_msgs/ # 消息和服务定义 + │ ├── msg/ + │ │ ├── TCPPose.msg # TCP 位姿消息 + │ │ └── JointState.msg # 关节状态消息 + │ └── srv/ + │ ├── MoveJoints.srv # 关节运动服务 + │ ├── MovePose.srv # 位姿运动服务 + │ ├── GetPose.srv # 查询位姿服务 + │ └── SetGripper.srv # 夹爪控制服务 + └── udp_teleop/ + ├── udp_teleop/ + │ ├── arm_control.py # 控制节点实现 + │ └── arm_control_client.py # 示例客户端 + └── config/ + └── arm_control.yaml # 参数配置 +``` + +## 🎯 常用命令 + +### 服务调用 + +```bash +# 1. 关节空间运动 +ros2 service call /arm_control/move_joints arm_control_msgs/srv/MoveJoints \ + "{height: -100, j2: 10, j3: 20, j4: 30, j5: 81, j6: 30, duration: 2.0}" + +# 2. 笛卡尔空间运动 +ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ + "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}" + +# 3. 查询当前位姿 +ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose + +# 4. 夹爪控制 +ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \ + "{grip: true}" +``` + +### 话题订阅 + +```bash +# 查看关节状态（10Hz 发布） +ros2 topic echo /arm_control/joint_states + +# 查看 TCP 位姿（10Hz 发布） +ros2 topic echo /arm_control/tcp_pose +``` + +### 调试命令 + +```bash +# 查看所有服务 +ros2 service list | grep arm_control + +# 查看所有话题 +ros2 topic list | grep arm_control + +# 查看节点信息 +ros2 node info /arm_control + +# 查看服务接口定义 +ros2 interface show arm_control_msgs/srv/MovePose +``` + +## 📝 Python 客户端模板 + +```python +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from arm_control_msgs.srv import MovePose, GetPose + +class MyController(Node): + def __init__(self): + super().__init__('my_controller') + + # 创建服务客户端 + self.move_cli = self.create_client(MovePose, 'arm_control/move_pose') + self.get_cli = self.create_client(GetPose, 'arm_control/get_pose') + + # 等待服务 + self.move_cli.wait_for_service() + self.get_cli.wait_for_service() + + def move_to(self, x, y, z, phi, duration=2.0): + """移动到指定位置""" + req = MovePose.Request() + req.x = x + req.y = y + req.z = z + req.phi = phi + req.duration = duration + + future = self.move_cli.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result().success + + def get_pose(self): + """查询当前位姿""" + req = GetPose.Request() + future = self.get_cli.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result() + +def main(): + rclpy.init() + controller = MyController() + + # 查询位姿 + pose = controller.get_pose() + print(f"当前位置: ({pose.x}, {pose.y}, {pose.z})") + + # 移动 + controller.move_to(200.0, 100.0, -100.0, 45.0) + + controller.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() +``` + +## 🔧 配置修改 + +编辑 `src/udp_teleop/config/arm_control.yaml`： + +```yaml +arm_control: + ros__parameters: + # 修改 ESP32 IP + udp_ip: '192.168.4.1' + + # 修改运动速度 + default_duration: 1.0 # 更快：0.5，更慢：2.0 + + # 修改关节限位 + height_min: -290 + height_max: 0 +``` + +修改后重新运行节点即可（无需重新编译）。 + +## ⚠️ 常见问题 + +### 编译失败 + +```bash +# 确保环境激活 +conda activate ros2_humble +source install/setup.bash + +# 清理后重新编译 +rm -rf build/ install/ log/ +./build_arm_control.sh +``` + +### 服务不可用 + +```bash +# 检查节点是否运行 +ros2 node list + +# 检查服务是否存在 +ros2 service list | grep arm_control + +# 查看节点日志 +ros2 run udp_teleop arm_control --ros-args --log-level debug +``` + +### UDP 连接失败 + +```bash +# 测试 UDP 连接 +echo 'XYW:0:0:0:XZHY' | nc -u 192.168.4.1 8888 + +# 修改 IP 配置 +ros2 run udp_teleop arm_control \ + --ros-args -p udp_ip:=<你的IP> -p udp_port:=8888 +``` + +## 📚 更多文档 + +- 完整使用文档：[ARM_CONTROL_README.md](ARM_CONTROL_README.md) +- 原始工具文档：[../tools/README.md](../tools/README.md) +- ROS 2 包文档：[src/udp_teleop/README.md](src/udp_teleop/README.md) + +## 🎓 下一步 + +1. **集成视觉**：创建视觉抓取节点，订阅相机话题，调用 arm_control 服务 +2. **添加规划**：使用 MoveIt 或自定义轨迹规划器 +3. **多机械臂**：启动多个 arm_control 节点控制多个机械臂 +4. **远程控制**：通过 ROS 2 的 DDS 实现跨机器控制 diff --git a/docs/VISION_GRASP_README.md b/docs/VISION_GRASP_README.md new file mode 100644 index 0000000..043073f --- /dev/null +++ b/docs/VISION_GRASP_README.md @@ -0,0 +1,359 @@ +# 视觉抓取节点使用指南 + +## 概述 + +`vision_grasp` 节点基于 `camera_to_base.py` 实现自动抓取和释放功能，将相机坐标系的检测结果转换为机械臂基坐标系，并自动执行抓取流程。 + +## 功能 + +### 1. 抓取功能 + +**输入**：相机坐标系 `(x, y, z)` + +**流程**： +1. 坐标转换：`(xc, yc, zc) = (x, -y, z)`（图像坐标到相机坐标） +2. 转换到基坐标系 +3. 释放夹爪（duration=0） +4. 移动到目标位置（duration=3s） +5. 抓取（duration=1s） +6. 回收到 (200, 0, 当前z) + +### 2. 释放功能 + +**输入**：基坐标系 `(x, y, z)` + +**流程**： +1. 移动到释放位置 +2. 释放夹爪（duration=0） +3. 回收到 (200, 0, 当前z) + +## 编译 + +```bash +cd ros2 +colcon build --packages-select udp_teleop +source install/setup.bash +``` + +## 运行 + +### 启动节点 + +**终端 1**：启动机械臂控制节点 +```bash +ros2 run udp_teleop arm_control \ + --ros-args --params-file src/udp_teleop/config/arm_control.yaml +``` + +**终端 2**：启动视觉抓取节点 +```bash +ros2 run udp_teleop vision_grasp \ + --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml +``` + +## 使用 + +### 方法 1：发布话题触发抓取 + +```bash +# 抓取：输入相机坐标 +ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \ + "{x: 10.0, y: 5.0, z: 250.0}" + +# 释放：输入基坐标 +ros2 topic pub --once /vision_grasp/release_target geometry_msgs/Point \ + "{x: 100.0, y: 150.0, z: -100.0}" +``` + +### 方法 2：Python 脚本集成 + +```python +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Point + +class VisionDetector(Node): + def __init__(self): + super().__init__('vision_detector') + + # 创建发布者 + self.grasp_pub = self.create_publisher( + Point, + 'vision_grasp/grasp_target', + 10 + ) + + def detect_and_grasp(self): + # 模拟检测结果（相机坐标系） + camera_x = 10.0 # 相机右侧 10mm + camera_y = 5.0 # 相机下方 5mm + camera_z = 250.0 # 前方 250mm + + # 发布抓取目标 + msg = Point() + msg.x = camera_x + msg.y = camera_y + msg.z = camera_z + + self.grasp_pub.publish(msg) + self.get_logger().info(f'发送抓取目标: ({camera_x}, {camera_y}, {camera_z})') + +def main(): + rclpy.init() + node = VisionDetector() + + # 检测并抓取 + node.detect_and_grasp() + + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() +``` + +### 方法 3：与检测节点集成 + +```python +#!/usr/bin/env python3 +"""完整的视觉检测+抓取示例""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from geometry_msgs.msg import Point +import cv2 +from cv_bridge import CvBridge + +class VisionPipeline(Node): + def __init__(self): + super().__init__('vision_pipeline') + + # 订阅相机图像 + self.image_sub = self.create_subscription( + Image, + '/camera/image_raw', + self.on_image, + 10 + ) + + # 发布抓取目标 + self.grasp_pub = self.create_publisher( + Point, + 'vision_grasp/grasp_target', + 10 + ) + + self.bridge = CvBridge() + + def on_image(self, msg): + # 转换 ROS 图像到 OpenCV + image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + + # 检测物体（示例：使用轮廓检测） + detected = self.detect_object(image) + + if detected: + camera_x, camera_y, camera_z = detected + + # 发布抓取目标 + target = Point() + target.x = camera_x + target.y = camera_y + target.z = camera_z + + self.grasp_pub.publish(target) + self.get_logger().info(f'检测到物体，发送抓取指令') + + def detect_object(self, image): + """检测物体并返回相机坐标""" + # TODO: 实现你的检测算法 + # 1. 图像处理（阈值、轮廓等） + # 2. 获取像素坐标 (u, v) 和像素宽度 + # 3. 使用相似三角形计算深度 + # 4. 转换到相机坐标系 + + # 示例返回值 + return (10.0, 5.0, 250.0) # (xc, yc, zc) + +def main(): + rclpy.init() + node = VisionPipeline() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() +``` + +## 参数配置 + +编辑 `config/vision_grasp.yaml`： + +```yaml +vision_grasp: + ros__parameters: + # 相机到 TCP 的变换（如果相机不在 TCP 中心） + cam_tx: 0.0 # X 偏移 + cam_ty: 0.0 # Y 偏移（高度） + cam_tz: 0.0 # Z 偏移（前后） + + # 回收位置 + retract_position_x: 200.0 + retract_position_y: 0.0 + + # 运动时长 + grasp_duration: 3.0 # 抓取移动时长 + release_duration: 2.0 # 释放移动时长 +``` + +## 坐标系说明 + +### 相机坐标系 + +``` + Yc (下) + | + | + o-----> Zc (前，水平) + / + / + Xc (右) +``` + +### 坐标转换 + +检测结果 `(x, y, z)` 表示： +- `x`: 图像列方向（右为正） +- `y`: 图像行方向（下为正） +- `z`: 深度方向（前为正） + +节点会自动转换： +``` +(xc, yc, zc) = (x, -y, z) +``` + +这是因为： +- 图像 Y 向下 → 相机 Y 向下（负号修正方向） +- 然后再转换到基坐标系 + +## 调试 + +### 查看节点状态 + +```bash +# 查看节点列表 +ros2 node list + +# 查看话题列表 +ros2 topic list | grep vision_grasp + +# 监听抓取目标 +ros2 topic echo /vision_grasp/grasp_target +``` + +### 测试流程 + +1. **启动节点** + ```bash + # 终端 1 + ros2 run udp_teleop arm_control --ros-args --params-file src/udp_teleop/config/arm_control.yaml + + # 终端 2 + ros2 run udp_teleop vision_grasp --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml + ``` + +2. **发送测试抓取** + ```bash + # 终端 3 + ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \ + "{x: 0.0, y: 0.0, z: 300.0}" + ``` + +3. **观察日志** + - 终端 2 会显示详细的抓取流程日志 + - 确认坐标转换和每一步动作 + +## 常见问题 + +### Q1: 坐标转换不正确 + +**检查**： +1. 相机内参是否准确标定 +2. 相机到 TCP 的变换参数是否正确 +3. 当前 TCP 位姿是否正确 + +### Q2: 抓取位置偏移 + +**可能原因**： +1. 深度计算不准确 +2. 相机安装角度有偏差 +3. 坐标系定义理解错误 + +**解决**： +1. 调整 `cam_pitch` 参数（如果相机有俯仰角） +2. 校准相机内参 +3. 使用已知位置物体验证 + +### Q3: 夹爪动作失败 + +**检查**： +1. arm_control 节点是否正常运行 +2. UDP 连接是否正常 +3. 关节限位是否合理 + +## 扩展功能 + +### 添加安全检查 + +```python +def execute_grasp(self, x: float, y: float, z: float, phi: float): + # 检查目标是否在工作空间内 + if not self.is_in_workspace(x, y, z): + self.get_logger().warn(f'目标超出工作空间: ({x}, {y}, {z})') + return + + # 执行抓取... +``` + +### 添加碰撞检测 + +```python +def is_path_safe(self, start, end): + # 检查路径是否安全 + # TODO: 实现碰撞检测逻辑 + return True +``` + +### 多物体抓取 + +```python +# 订阅物体列表 +self.objects_sub = self.create_subscription( + PointArray, # 自定义消息类型 + 'vision_grasp/object_list', + self.handle_objects, + 10 +) + +def handle_objects(self, msg): + for obj in msg.points: + self.execute_grasp(obj.x, obj.y, obj.z, self.current_phi) + # 等待完成... +``` + +## 相关文件 + +- 节点实现：`udp_teleop/vision_grasp.py` +- 配置文件：`udp_teleop/config/vision_grasp.yaml` +- 坐标变换工具：`tools/camera_to_base.py` +- 机械臂控制：`udp_teleop/arm_control.py` + +## 下一步 + +1. 集成物体检测算法（YOLO、轮廓检测等） +2. 添加深度估计（相似三角形、双目视觉等） +3. 优化抓取策略（多物体排序、路径规划等） +4. 添加可视化（RViz 显示检测结果和机械臂状态） diff --git a/docs/VISION_GRASP_SUMMARY.md b/docs/VISION_GRASP_SUMMARY.md new file mode 100644 index 0000000..89f1426 --- /dev/null +++ b/docs/VISION_GRASP_SUMMARY.md @@ -0,0 +1,286 @@ +# 视觉抓取节点 - 完成总结 + +## ✅ 完成的工作 + +### 1. 创建了视觉抓取 ROS 节点 (`vision_grasp.py`) + +**功能**： +- ✅ 抓取功能：输入相机坐标 → 自动转换 → 执行抓取流程 +- ✅ 释放功能：输入基坐标 → 移动 → 释放物体 +- ✅ 坐标变换：集成 `camera_to_base.py` 的完整变换逻辑 +- ✅ 自动化流程：释放夹爪 → 移动 → 抓取 → 回收 + +### 2. 抓取流程 + +``` +输入相机坐标 (x, y, z) + ↓ +转换: (xc, yc, zc) = (x, -y, z) + ↓ +变换到基坐标系 + ↓ +1. Release 夹爪 (duration=0) + ↓ +2. 移动到目标 (duration=3s) + ↓ +3. Grip 夹爪 (duration=1s) + ↓ +4. 回收到 (200, 0, 当前z) +``` + +### 3. 释放流程 + +``` +输入基坐标 (x, y, z) + ↓ +1. 移动到释放位置 + ↓ +2. Release 夹爪 (duration=0) + ↓ +3. 回收到 (200, 0, 当前z) +``` + +## 📁 创建的文件 + +``` +ros2/ +├── src/udp_teleop/ +│ ├── udp_teleop/ +│ │ └── vision_grasp.py ✨ 视觉抓取节点 +│ └── config/ +│ └── vision_grasp.yaml ✨ 参数配置 +├── test_vision_grasp.py ✨ 测试脚本 +└── VISION_GRASP_README.md ✨ 完整文档 +``` + +## 🚀 快速使用 + +### 启动节点 + +**终端 1**：arm_control 节点 +```bash +cd ros2 +source install/setup.bash + +ros2 run udp_teleop arm_control \ + --ros-args --params-file src/udp_teleop/config/arm_control.yaml +``` + +**终端 2**：vision_grasp 节点 +```bash +cd ros2 +source install/setup.bash + +ros2 run udp_teleop vision_grasp \ + --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml +``` + +**终端 3**：测试 +```bash +cd ros2 +source install/setup.bash + +# 测试抓取（相机正前方 300mm） +python test_vision_grasp.py grasp 0 0 300 + +# 测试抓取（相机右侧 50mm，前方 300mm） +python test_vision_grasp.py grasp 50 0 300 + +# 测试释放（基坐标） +python test_vision_grasp.py release 100 150 -100 +``` + +### 或使用话题发布 + +```bash +# 抓取 +ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \ + "{x: 0.0, y: 0.0, z: 300.0}" + +# 释放 +ros2 topic pub --once /vision_grasp/release_target geometry_msgs/Point \ + "{x: 100.0, y: 150.0, z: -100.0}" +``` + +## 🎯 关键特性 + +### 1. 自动坐标转换 + +- **输入**：相机坐标系 `(x, y, z)` +- **自动转换**：`(xc, yc, zc) = (x, -y, z)`（图像坐标修正） +- **变换到基坐标系**：使用当前 TCP 位姿进行完整变换 + +### 2. 参数化配置 + +```yaml +vision_grasp: + ros__parameters: + # 相机到 TCP 的变换 + cam_tx: 0.0 + cam_ty: 0.0 + cam_tz: 0.0 + + # 回收位置 + retract_position_x: 200.0 + retract_position_y: 0.0 + + # 运动时长 + grasp_duration: 3.0 + release_duration: 2.0 +``` + +### 3. 完整日志 + +节点会输出详细的流程日志： +``` +============================================================ +开始抓取流程 +============================================================ +1. 释放夹爪 +2. 移动到目标位置: (323.5, 229.6, -108.6) +3. 抓取物体 +4. 移动到回收位置: (200.0, 0.0, -108.6) +============================================================ +✓ 抓取完成！ +============================================================ +``` + +## 🔗 集成示例 + +### Python 脚本集成 + +```python +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Point + +class MyDetector(Node): + def __init__(self): + super().__init__('my_detector') + self.grasp_pub = self.create_publisher( + Point, 'vision_grasp/grasp_target', 10) + + def on_detection(self, camera_x, camera_y, camera_z): + """检测到物体后触发抓取""" + msg = Point() + msg.x = camera_x + msg.y = camera_y + msg.z = camera_z + self.grasp_pub.publish(msg) + +def main(): + rclpy.init() + node = MyDetector() + + # 模拟检测结果 + node.on_detection(10.0, 5.0, 250.0) + + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() +``` + +## 📊 话题接口 + +| 话题 | 类型 | 说明 | +|------|------|------| +| `/vision_grasp/grasp_target` | geometry_msgs/Point | 抓取目标（相机坐标） | +| `/vision_grasp/release_target` | geometry_msgs/Point | 释放目标（基坐标） | + +## 🎓 下一步 + +### 1. 集成物体检测 + +```python +# 订阅相机图像 +self.image_sub = self.create_subscription( + Image, '/camera/image_raw', self.on_image, 10) + +def on_image(self, msg): + # 检测物体 + camera_x, camera_y, camera_z = detect_object(msg) + + # 触发抓取 + self.publish_grasp_target(camera_x, camera_y, camera_z) +``` + +### 2. 添加深度估计 + +使用 `tools/vision_transform.py` 中的相似三角形方法： + +```python +from tools.vision_transform import compute_depth_from_size + +# 从检测获得像素宽度 +pixel_width = 100 # px +real_width = 50 # mm +focal_length = 500 # px + +depth = compute_depth_from_size(pixel_width, real_width, focal_length) +``` + +### 3. 多物体抓取 + +```python +# 创建队列 +self.grasp_queue = [] + +def on_multiple_detections(self, detections): + for det in detections: + self.grasp_queue.append(det) + + # 逐个抓取 + while self.grasp_queue: + target = self.grasp_queue.pop(0) + self.publish_grasp_target(target.x, target.y, target.z) + # 等待完成... +``` + +## 🐛 故障排查 + +### Q1: 坐标转换不正确 + +**检查**： +1. TCP 位姿是否正确（`ros2 service call /arm_control/get_pose`） +2. 相机到 TCP 的变换参数（`cam_tx/ty/tz`, `cam_roll/pitch/yaw`） +3. 坐标系方向理解是否正确 + +### Q2: 抓取位置偏移 + +**解决**： +1. 校准相机内参 +2. 验证深度计算准确性 +3. 调整 `cam_pitch`（如果相机有俯仰角） + +### Q3: 服务调用超时 + +**检查**： +1. arm_control 节点是否运行 +2. UDP 连接是否正常 +3. 机械臂是否在合理位置 + +## 📚 相关文档 + +- **完整文档**：`VISION_GRASP_README.md` +- **坐标变换**：`tools/camera_to_base.py` +- **机械臂控制**：`ARM_CONTROL_README.md` +- **视觉变换**：`docs/vision_calibration_horizontal.md` + +## 🎉 总结 + +现在你有了一个完整的视觉抓取系统： + +1. ✅ **独立的机械臂控制节点** - `arm_control` +2. ✅ **自动化抓取节点** - `vision_grasp` +3. ✅ **完整的坐标变换** - 相机 → 基坐标系 +4. ✅ **参数化配置** - 灵活调整参数 +5. ✅ **测试工具** - 快速验证功能 +6. ✅ **完整文档** - 使用指南和示例 + +只需要： +1. 添加物体检测算法 +2. 连接相机获取图像 +3. 发布检测结果到 `/vision_grasp/grasp_target` + +系统就会自动完成抓取！ diff --git a/docs/craic.md b/docs/arm.md similarity index 100% rename from docs/craic.md rename to docs/arm.md diff --git a/ros2/src/arm_control_msgs/CMakeLists.txt b/ros2/src/arm_control_msgs/CMakeLists.txt new file mode 100644 index 0000000..4072dba --- /dev/null +++ b/ros2/src/arm_control_msgs/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.10) +project(arm_control_msgs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +# Generate messages and services +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/TCPPose.msg" + "msg/JointState.msg" + "srv/MoveJoints.srv" + "srv/MovePose.srv" + "srv/GetPose.srv" + "srv/SetGripper.srv" + DEPENDENCIES std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/ros2/src/arm_control_msgs/msg/JointState.msg b/ros2/src/arm_control_msgs/msg/JointState.msg new file mode 100644 index 0000000..69710c1 --- /dev/null +++ b/ros2/src/arm_control_msgs/msg/JointState.msg @@ -0,0 +1,8 @@ +# 关节状态消息 +std_msgs/Header header +int32 height # 高度 (mm) +int32 j2 # 关节 2 角度 (度) +int32 j3 # 关节 3 角度 (度) +int32 j4 # 关节 4 角度 (度) +int32 j5 # 关节 5 角度 (度) +int32 j6 # 关节 6 角度 (度) diff --git a/ros2/src/arm_control_msgs/msg/TCPPose.msg b/ros2/src/arm_control_msgs/msg/TCPPose.msg new file mode 100644 index 0000000..1eed2f7 --- /dev/null +++ b/ros2/src/arm_control_msgs/msg/TCPPose.msg @@ -0,0 +1,6 @@ +# TCP 位姿消息 +std_msgs/Header header +float64 x # X 坐标 (mm) +float64 y # Y 坐标 (mm) +float64 z # Z 坐标 (mm) +float64 phi # 偏航角 (度) diff --git a/ros2/src/arm_control_msgs/package.xml b/ros2/src/arm_control_msgs/package.xml new file mode 100644 index 0000000..ea88616 --- /dev/null +++ b/ros2/src/arm_control_msgs/package.xml @@ -0,0 +1,22 @@ + + + + arm_control_msgs + 0.0.1 + Message and service definitions for arm control + fallensigh + MIT + + ament_cmake + rosidl_default_generators + + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/ros2/src/arm_control_msgs/srv/GetPose.srv b/ros2/src/arm_control_msgs/srv/GetPose.srv new file mode 100644 index 0000000..6cf1bfd --- /dev/null +++ b/ros2/src/arm_control_msgs/srv/GetPose.srv @@ -0,0 +1,14 @@ +# 查询当前位姿服务 +--- +bool success # 是否成功 +string message # 返回信息 +float64 x # 当前 X 坐标 (mm) +float64 y # 当前 Y 坐标 (mm) +float64 z # 当前 Z 坐标 (mm) +float64 phi # 当前偏航角 (度) +int32 height # 当前高度 (mm) +int32 j2 # 当前 J2 角度 (度) +int32 j3 # 当前 J3 角度 (度) +int32 j4 # 当前 J4 角度 (度) +int32 j5 # 当前 J5 角度 (度) +int32 j6 # 当前 J6 角度 (度) diff --git a/ros2/src/arm_control_msgs/srv/MoveJoints.srv b/ros2/src/arm_control_msgs/srv/MoveJoints.srv new file mode 100644 index 0000000..5f1909d --- /dev/null +++ b/ros2/src/arm_control_msgs/srv/MoveJoints.srv @@ -0,0 +1,11 @@ +# 关节空间运动服务 +int32 height # 目标高度 (mm) +int32 j2 # 目标 J2 角度 (度) +int32 j3 # 目标 J3 角度 (度) +int32 j4 # 目标 J4 角度 (度) +int32 j5 # 目标 J5 角度 (度) +int32 j6 # 目标 J6 角度 (度) +float64 duration # 运动时长 (秒，0 表示使用默认值) +--- +bool success # 是否成功 +string message # 返回信息 diff --git a/ros2/src/arm_control_msgs/srv/MovePose.srv b/ros2/src/arm_control_msgs/srv/MovePose.srv new file mode 100644 index 0000000..4e0ec31 --- /dev/null +++ b/ros2/src/arm_control_msgs/srv/MovePose.srv @@ -0,0 +1,18 @@ +# 笛卡尔空间运动服务 +float64 x # 目标 X 坐标 (mm) +float64 y # 目标 Y 坐标 (mm) +float64 z # 目标 Z 坐标 (mm) +float64 phi # 目标偏航角 (度) +bool elbow_up # 是否使用肘部向上解 +uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合 +bool grip # 是否抓取 +bool release # 是否释放 +float64 duration # 运动时长 (秒，0 表示使用默认值) + +# 夹爪状态常量 +uint8 GRIPPER_KEEP = 0 +uint8 GRIPPER_OPEN = 1 +uint8 GRIPPER_CLOSED = 2 +--- +bool success # 是否成功 +string message # 返回信息 diff --git a/ros2/src/arm_control_msgs/srv/SetGripper.srv b/ros2/src/arm_control_msgs/srv/SetGripper.srv new file mode 100644 index 0000000..918bb07 --- /dev/null +++ b/ros2/src/arm_control_msgs/srv/SetGripper.srv @@ -0,0 +1,12 @@ +# 夹爪控制服务 +uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合 +bool grip # 是否抓取 +bool release # 是否释放 + +# 夹爪状态常量 +uint8 GRIPPER_KEEP = 0 +uint8 GRIPPER_OPEN = 1 +uint8 GRIPPER_CLOSED = 2 +--- +bool success # 是否成功 +string message # 返回信息 diff --git a/ros2/src/udp_teleop/config/arm_control.yaml b/ros2/src/udp_teleop/config/arm_control.yaml new file mode 100644 index 0000000..fe3de57 --- /dev/null +++ b/ros2/src/udp_teleop/config/arm_control.yaml @@ -0,0 +1,38 @@ +# 机械臂控制节点参数配置 + +arm_control: + ros__parameters: + # UDP 通信配置 + udp_ip: '192.168.4.1' + udp_port: 8888 + + # 机械臂几何参数 (mm) + l1: 125.0 # J2-J3 连杆长度 + l2: 125.0 # J3-J4 连杆长度 + x4: 110.0 # J4-TCP 水平偏移 + z4: 80.0 # J4-TCP 垂直偏移 + + # 关节限位 (mm 或 度) + height_min: -290 + height_max: 0 + j2_min: -110 + j2_max: 115 + j3_min: -120 + j3_max: 145 + j4_min: -90 + j4_max: 130 + joint_min: -180 # J5/J6 通用限位 + joint_max: 180 + + # 零点偏移 (度) + zero_j2: 3 + zero_j3: 7 + zero_j4: 25 + + # 运动参数 + default_duration: 3.0 # 默认运动时长 (秒) + default_rate: 100.0 # 插值频率 (Hz) + use_state_cache: true # 是否使用状态缓存 + + # 状态发布频率 + publish_rate: 10.0 # Hz diff --git a/ros2/src/udp_teleop/config/vision_grasp.yaml b/ros2/src/udp_teleop/config/vision_grasp.yaml new file mode 100644 index 0000000..20ebc39 --- /dev/null +++ b/ros2/src/udp_teleop/config/vision_grasp.yaml @@ -0,0 +1,21 @@ +# 视觉抓取节点参数配置 + +vision_grasp: + ros__parameters: + # 相机到 TCP 的变换参数 + cam_tx: 0.0 # X 平移 (mm) + cam_ty: 0.0 # Y 平移 (mm) + cam_tz: 0.0 # Z 平移 (mm) + cam_roll: 0.0 # 绕 X 轴旋转 (度) + cam_pitch: 0.0 # 绕 Y 轴旋转 (度) + cam_yaw: 0.0 # 绕 Z 轴旋转 (度) + + # 抓取参数 + approach_height_offset: 50.0 # 接近高度偏移 (mm) + retract_position_x: 200.0 # 回收位置 X (mm) + retract_position_y: 0.0 # 回收位置 Y (mm) + + # 运动时长 + grasp_duration: 3.0 # 抓取移动时长 (秒) + release_duration: 2.0 # 释放移动时长 (秒) + gripper_duration: 1.0 # 夹爪动作时长 (秒) diff --git a/ros2/src/udp_teleop/package.xml b/ros2/src/udp_teleop/package.xml index 501df2c..9ede626 100644 --- a/ros2/src/udp_teleop/package.xml +++ b/ros2/src/udp_teleop/package.xml @@ -3,9 +3,13 @@ udp_teleop 0.0.0 - TODO: Package description + UDP teleoperation and arm control for CRAIC robot fallensigh - TODO: License declaration + MIT + + rclpy + std_msgs + arm_control_msgs ament_copyright ament_flake8 diff --git a/ros2/src/udp_teleop/setup.py b/ros2/src/udp_teleop/setup.py index f2a4c26..a3c5138 100644 --- a/ros2/src/udp_teleop/setup.py +++ b/ros2/src/udp_teleop/setup.py @@ -29,7 +29,9 @@ setup( }, entry_points={ 'console_scripts': [ - 'keyboard_control = udp_teleop.keyboard_control:main' + 'keyboard_control = udp_teleop.keyboard_control:main', + 'arm_control = udp_teleop.arm_control:main', + 'vision_grasp = udp_teleop.vision_grasp:main' ], }, ) diff --git a/ros2/src/udp_teleop/udp_teleop/arm_control.py b/ros2/src/udp_teleop/udp_teleop/arm_control.py new file mode 100644 index 0000000..03cf8dc --- /dev/null +++ b/ros2/src/udp_teleop/udp_teleop/arm_control.py @@ -0,0 +1,743 @@ +#!/usr/bin/env python3 +"""机械臂控制 ROS 节点（独立版本） + +完全独立，不依赖 udp_control.py +包含所有必要的运动学和控制代码 +""" + +import json +import math +import socket +import time +from dataclasses import dataclass +from pathlib import Path +from typing import Optional, List + +import rclpy +from rclpy.node import Node + +# 导入自定义消息 +from arm_control_msgs.srv import ( + MoveJoints, + MovePose, + GetPose, + SetGripper +) +from arm_control_msgs.msg import ( + JointState, + TCPPose +) + +# ============================================================================ +# 常量定义 +# ============================================================================ + +DEFAULT_UDP_IP = "192.168.4.1" +DEFAULT_UDP_PORT = 8888 + +DEFAULT_HEIGHT_MIN = -290 +DEFAULT_HEIGHT_MAX = 0 +DEFAULT_JOINT_MIN = -180 +DEFAULT_JOINT_MAX = 180 +DEFAULT_J2_MIN = -110 +DEFAULT_J2_MAX = 115 +DEFAULT_J3_MIN = -120 +DEFAULT_J3_MAX = 145 +DEFAULT_J4_MIN = -90 +DEFAULT_J4_MAX = 130 + +J5_OPEN = 81 +J5_CLOSED = -100 +DEFAULT_FIXED_J5 = J5_OPEN + +GRIP_ANGLE = -5 +RELEASE_ANGLE = 80 +DEFAULT_FIXED_J6 = RELEASE_ANGLE + +DEFAULT_ZERO_J2 = 3 +DEFAULT_ZERO_J3 = 7 +DEFAULT_ZERO_J4 = 25 + +DEFAULT_L1 = 125.0 +DEFAULT_L2 = 125.0 +DEFAULT_X4 = 110.0 +DEFAULT_Z4 = 80.0 + +DEFAULT_INTERP_DURATION = 1.0 +DEFAULT_INTERP_RATE = 20.0 + +STATE_FILE = Path.home() / ".ros" / "udp_control_state.json" + +# ============================================================================ +# 数据类定义 +# ============================================================================ + +class ArmControlError(ValueError): + """机械臂控制错误""" + pass + + +@dataclass(frozen=True) +class ArmGeometry: + """机械臂几何参数""" + l1: float + l2: float + x4: float + z4: float + + +@dataclass(frozen=True) +class ArmLimits: + """关节限位""" + height_min: int = DEFAULT_HEIGHT_MIN + height_max: int = DEFAULT_HEIGHT_MAX + joint_min: int = DEFAULT_JOINT_MIN + joint_max: int = DEFAULT_JOINT_MAX + j2_min: int = DEFAULT_J2_MIN + j2_max: int = DEFAULT_J2_MAX + j3_min: int = DEFAULT_J3_MIN + j3_max: int = DEFAULT_J3_MAX + j4_min: int = DEFAULT_J4_MIN + j4_max: int = DEFAULT_J4_MAX + + +@dataclass(frozen=True) +class ArmZeroOffsets: + """零点偏移""" + j2: int = DEFAULT_ZERO_J2 + j3: int = DEFAULT_ZERO_J3 + j4: int = DEFAULT_ZERO_J4 + + +@dataclass(frozen=True) +class ArmJointState: + """关节状态""" + height: int + j2: int + j3: int + j4: int + j5: int = DEFAULT_FIXED_J5 + j6: int = DEFAULT_FIXED_J6 + + def to_udp_message(self) -> bytes: + """转换为 UDP 消息""" + return ( + f"JXB:{self.height}:{self.j2}:{self.j3}:{self.j4}:" + f"{self.j5}:{self.j6}:0:0:EZHY\n" + ).encode("utf-8") + + +@dataclass(frozen=True) +class ArmPose: + """TCP 位姿""" + x: float + y: float + z: float + phi_deg: float + + +@dataclass(frozen=True) +class ArmMathState: + """数学坐标系的关节状态""" + d1: float + theta2_deg: float + theta3_deg: float + theta4_deg: float + + +# ============================================================================ +# 运动学函数 +# ============================================================================ + +def clamp_int(value: float, lower: int, upper: int, name: str) -> int: + """限幅并转换为整数""" + rounded = int(round(value)) + if rounded < lower or rounded > upper: + raise ArmControlError(f"{name}={rounded} 超出范围 [{lower}, {upper}]") + return rounded + + +def normalize_angle_deg(angle_deg: float) -> float: + """角度归一化到 [-180, 180)""" + normalized = (angle_deg + 180.0) % 360.0 - 180.0 + if normalized == -180.0 and angle_deg > 0: + return 180.0 + return normalized + + +def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose: + """正运动学：关节角度 → TCP 位姿""" + theta2 = math.radians(state.theta2_deg) + theta3 = math.radians(state.theta3_deg) + theta4 = math.radians(state.theta4_deg) + phi = theta2 + theta3 + theta4 + + j4_center_x = ( + geometry.l1 * math.cos(theta2) + + geometry.l2 * math.cos(theta2 + theta3) + ) + j4_center_y = ( + geometry.l1 * math.sin(theta2) + + geometry.l2 * math.sin(theta2 + theta3) + ) + + x = j4_center_x + geometry.x4 * math.cos(phi) + y = j4_center_y + geometry.x4 * math.sin(phi) + z = state.d1 - geometry.z4 + + return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi)) + + +def inverse_kinematics( + geometry: ArmGeometry, + pose: ArmPose, + limits: ArmLimits, + elbow_up: bool, + j5: int, + j6: int, +) -> ArmMathState: + """逆运动学：TCP 位姿 → 关节角度""" + # 计算 J4 中心位置 + phi = math.radians(pose.phi_deg) + j4_x = pose.x - geometry.x4 * math.cos(phi) + j4_y = pose.y - geometry.x4 * math.sin(phi) + j4_z = pose.z + geometry.z4 + + d1 = j4_z + + # 计算平面距离 + r2 = j4_x * j4_x + j4_y * j4_y + if r2 < 1e-9: + raise ArmControlError("目标点过于接近奇异点") + + # 计算 theta3 + denom = 2.0 * geometry.l1 * geometry.l2 + if abs(denom) < 1e-9: + raise ArmControlError("几何参数无效") + + c3 = (r2 - geometry.l1 * geometry.l1 - geometry.l2 * geometry.l2) / denom + if c3 < -1.0 - 1e-9 or c3 > 1.0 + 1e-9: + reach = math.sqrt(r2) + raise ArmControlError(f"目标超出工作空间，距离={reach:.3f}mm") + c3 = max(-1.0, min(1.0, c3)) + + s3_abs = math.sqrt(max(0.0, 1.0 - c3 * c3)) + s3 = -s3_abs if elbow_up else s3_abs + + theta3 = math.atan2(s3, c3) + theta2 = math.atan2(j4_y, j4_x) - math.atan2( + geometry.l2 * s3, + geometry.l1 + geometry.l2 * c3, + ) + theta4 = phi - theta2 - theta3 + + return ArmMathState( + d1=d1, + theta2_deg=normalize_angle_deg(math.degrees(theta2)), + theta3_deg=normalize_angle_deg(math.degrees(theta3)), + theta4_deg=normalize_angle_deg(math.degrees(theta4)), + ) + + +def command_to_math_state( + command_state: ArmJointState, + zero_offsets: ArmZeroOffsets, +) -> ArmMathState: + """命令状态 → 数学状态""" + return ArmMathState( + d1=command_state.height, + theta2_deg=command_state.j2 - zero_offsets.j2, + theta3_deg=command_state.j3 - zero_offsets.j3, + theta4_deg=command_state.j4 - zero_offsets.j4, + ) + + +def math_to_command_state( + math_state: ArmMathState, + zero_offsets: ArmZeroOffsets, + limits: ArmLimits, + j5: int, + j6: int, +) -> ArmJointState: + """数学状态 → 命令状态""" + return ArmJointState( + height=clamp_int( + round(math_state.d1), + limits.height_min, + limits.height_max, + "height", + ), + j2=clamp_int( + math_state.theta2_deg + zero_offsets.j2, + limits.j2_min, + limits.j2_max, + "J2", + ), + j3=clamp_int( + math_state.theta3_deg + zero_offsets.j3, + limits.j3_min, + limits.j3_max, + "J3", + ), + j4=clamp_int( + math_state.theta4_deg + zero_offsets.j4, + limits.j4_min, + limits.j4_max, + "J4", + ), + j5=clamp_int(j5, limits.joint_min, limits.joint_max, "J5"), + j6=clamp_int(j6, limits.joint_min, limits.joint_max, "J6"), + ) + + +def interpolate_command_states( + start: ArmJointState, + end: ArmJointState, + steps: int, +) -> List[ArmJointState]: + """关节空间插值""" + if steps <= 1: + return [end] + + states = [] + for step_index in range(1, steps + 1): + t = step_index / steps + states.append( + ArmJointState( + height=int(round(start.height + (end.height - start.height) * t)), + j2=int(round(start.j2 + (end.j2 - start.j2) * t)), + j3=int(round(start.j3 + (end.j3 - start.j3) * t)), + j4=int(round(start.j4 + (end.j4 - start.j4) * t)), + j5=int(round(start.j5 + (end.j5 - start.j5) * t)), + j6=int(round(start.j6 + (end.j6 - start.j6) * t)), + ) + ) + return states + + +# ============================================================================ +# ROS 节点 +# ============================================================================ + +class ArmControlNode(Node): + """机械臂控制 ROS 节点""" + + def __init__(self): + super().__init__('arm_control') + + # 声明参数 + self.declare_parameters( + namespace='', + parameters=[ + ('udp_ip', DEFAULT_UDP_IP), + ('udp_port', DEFAULT_UDP_PORT), + ('l1', DEFAULT_L1), + ('l2', DEFAULT_L2), + ('x4', DEFAULT_X4), + ('z4', DEFAULT_Z4), + ('height_min', DEFAULT_HEIGHT_MIN), + ('height_max', DEFAULT_HEIGHT_MAX), + ('j2_min', DEFAULT_J2_MIN), + ('j2_max', DEFAULT_J2_MAX), + ('j3_min', DEFAULT_J3_MIN), + ('j3_max', DEFAULT_J3_MAX), + ('j4_min', DEFAULT_J4_MIN), + ('j4_max', DEFAULT_J4_MAX), + ('joint_min', DEFAULT_JOINT_MIN), + ('joint_max', DEFAULT_JOINT_MAX), + ('zero_j2', DEFAULT_ZERO_J2), + ('zero_j3', DEFAULT_ZERO_J3), + ('zero_j4', DEFAULT_ZERO_J4), + ('default_duration', DEFAULT_INTERP_DURATION), + ('default_rate', DEFAULT_INTERP_RATE), + ('use_state_cache', True), + ('publish_rate', 10.0), + ] + ) + + # 获取参数 + self.udp_ip = self.get_parameter('udp_ip').value + self.udp_port = self.get_parameter('udp_port').value + self.publish_rate = self.get_parameter('publish_rate').value + + # 机械臂几何参数 + self.geometry = ArmGeometry( + l1=self.get_parameter('l1').value, + l2=self.get_parameter('l2').value, + x4=self.get_parameter('x4').value, + z4=self.get_parameter('z4').value, + ) + + # 关节限位 + self.limits = ArmLimits( + height_min=self.get_parameter('height_min').value, + height_max=self.get_parameter('height_max').value, + j2_min=self.get_parameter('j2_min').value, + j2_max=self.get_parameter('j2_max').value, + j3_min=self.get_parameter('j3_min').value, + j3_max=self.get_parameter('j3_max').value, + j4_min=self.get_parameter('j4_min').value, + j4_max=self.get_parameter('j4_max').value, + joint_min=self.get_parameter('joint_min').value, + joint_max=self.get_parameter('joint_max').value, + ) + + # 零点偏移 + self.zero_offsets = ArmZeroOffsets( + j2=self.get_parameter('zero_j2').value, + j3=self.get_parameter('zero_j3').value, + j4=self.get_parameter('zero_j4').value, + ) + + # 默认插值参数 + self.default_duration = self.get_parameter('default_duration').value + self.default_rate = self.get_parameter('default_rate').value + self.use_state_cache = self.get_parameter('use_state_cache').value + + # 当前状态 + self.current_state: Optional[ArmJointState] = None + self.load_state() + + # UDP socket + self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + + # 创建服务 + self.srv_move_joints = self.create_service( + MoveJoints, + 'arm_control/move_joints', + self.handle_move_joints + ) + self.srv_move_pose = self.create_service( + MovePose, + 'arm_control/move_pose', + self.handle_move_pose + ) + self.srv_get_pose = self.create_service( + GetPose, + 'arm_control/get_pose', + self.handle_get_pose + ) + self.srv_set_gripper = self.create_service( + SetGripper, + 'arm_control/set_gripper', + self.handle_set_gripper + ) + + # 创建发布者 + self.pub_joint_states = self.create_publisher( + JointState, + 'arm_control/joint_states', + 10 + ) + self.pub_tcp_pose = self.create_publisher( + TCPPose, + 'arm_control/tcp_pose', + 10 + ) + + # 创建定时器发布状态 + self.timer = self.create_timer( + 1.0 / self.publish_rate, + self.publish_state + ) + + self.get_logger().info(f'机械臂控制节点已启动') + self.get_logger().info(f'UDP 目标: {self.udp_ip}:{self.udp_port}') + + def load_state(self): + """从缓存加载状态""" + if not self.use_state_cache or not STATE_FILE.exists(): + self.current_state = ArmJointState( + height=0, + j2=self.zero_offsets.j2, + j3=self.zero_offsets.j3, + j4=self.zero_offsets.j4, + j5=DEFAULT_FIXED_J5, + j6=DEFAULT_FIXED_J6, + ) + return + + try: + data = json.loads(STATE_FILE.read_text()) + self.current_state = ArmJointState( + height=data['height'], + j2=data['j2'], + j3=data['j3'], + j4=data['j4'], + j5=data.get('j5', DEFAULT_FIXED_J5), + j6=data.get('j6', DEFAULT_FIXED_J6), + ) + self.get_logger().info(f'从缓存加载状态') + except Exception as e: + self.get_logger().warn(f'加载状态失败: {e}，使用默认状态') + self.current_state = ArmJointState( + height=0, + j2=self.zero_offsets.j2, + j3=self.zero_offsets.j3, + j4=self.zero_offsets.j4, + j5=DEFAULT_FIXED_J5, + j6=DEFAULT_FIXED_J6, + ) + + def save_state(self): + """保存状态到缓存""" + if not self.use_state_cache or self.current_state is None: + return + + try: + STATE_FILE.parent.mkdir(parents=True, exist_ok=True) + data = { + 'height': self.current_state.height, + 'j2': self.current_state.j2, + 'j3': self.current_state.j3, + 'j4': self.current_state.j4, + 'j5': self.current_state.j5, + 'j6': self.current_state.j6, + } + STATE_FILE.write_text(json.dumps(data, indent=2)) + except Exception as e: + self.get_logger().warn(f'保存状态失败: {e}') + + def send_udp_commands( + self, + states: List[ArmJointState], + duration: float + ) -> bool: + """发送 UDP 命令序列""" + if not states: + return True + + delay = duration / len(states) if len(states) > 1 and duration > 0.0 else 0.0 + + try: + for i, state in enumerate(states): + msg = state.to_udp_message() + self.udp_socket.sendto(msg, (self.udp_ip, self.udp_port)) + + if delay > 0.0 and i < len(states) - 1: + time.sleep(delay) + + self.current_state = states[-1] + self.save_state() + return True + + except Exception as e: + self.get_logger().error(f'发送 UDP 命令失败: {e}') + return False + + + def handle_move_joints(self, request, response): + """处理关节空间运动服务""" + try: + target_state = ArmJointState( + height=clamp_int(request.height, self.limits.height_min, self.limits.height_max, 'height'), + j2=clamp_int(request.j2, self.limits.j2_min, self.limits.j2_max, 'j2'), + j3=clamp_int(request.j3, self.limits.j3_min, self.limits.j3_max, 'j3'), + j4=clamp_int(request.j4, self.limits.j4_min, self.limits.j4_max, 'j4'), + j5=clamp_int(request.j5, self.limits.joint_min, self.limits.joint_max, 'j5'), + j6=clamp_int(request.j6, self.limits.joint_min, self.limits.joint_max, 'j6'), + ) + + duration = request.duration if request.duration > 0 else self.default_duration + steps = max(1, int(math.ceil(duration * self.default_rate))) + + path = interpolate_command_states(self.current_state, target_state, steps) + success = self.send_udp_commands(path, duration) + + response.success = success + response.message = "运动完成" if success else "运动失败" + self.get_logger().info(f'关节运动 -> {response.message}') + + except Exception as e: + response.success = False + response.message = f'错误: {str(e)}' + self.get_logger().error(f'关节运动失败: {e}') + + return response + + def handle_move_pose(self, request, response): + """处理笛卡尔空间运动服务""" + try: + target_pose = ArmPose( + x=request.x, + y=request.y, + z=request.z, + phi_deg=request.phi + ) + + # 解析夹爪状态 + if request.gripper_state == SetGripper.Request.GRIPPER_OPEN: + j5 = J5_OPEN + elif request.gripper_state == SetGripper.Request.GRIPPER_CLOSED: + j5 = J5_CLOSED + else: + j5 = self.current_state.j5 + + if request.grip: + j6 = GRIP_ANGLE + elif request.release: + j6 = RELEASE_ANGLE + else: + j6 = self.current_state.j6 + + # 逆运动学 + math_state = inverse_kinematics( + geometry=self.geometry, + pose=target_pose, + limits=self.limits, + elbow_up=request.elbow_up, + j5=j5, + j6=j6, + ) + + # 转换为命令状态 + target_state = math_to_command_state( + math_state, + self.zero_offsets, + self.limits, + j5=j5, + j6=j6, + ) + + duration = request.duration if request.duration > 0 else self.default_duration + steps = max(1, int(math.ceil(duration * self.default_rate))) + + path = interpolate_command_states(self.current_state, target_state, steps) + success = self.send_udp_commands(path, duration) + + response.success = success + response.message = "运动完成" if success else "运动失败" + self.get_logger().info( + f'位姿运动: ({request.x:.1f}, {request.y:.1f}, {request.z:.1f}, {request.phi:.1f}) ' + f'-> {response.message}' + ) + + except Exception as e: + response.success = False + response.message = f'错误: {str(e)}' + self.get_logger().error(f'位姿运动失败: {e}') + + return response + + def handle_get_pose(self, request, response): + """处理查询位姿服务""" + try: + if self.current_state is None: + response.success = False + response.message = "当前状态未初始化" + return response + + math_state = command_to_math_state(self.current_state, self.zero_offsets) + pose = forward_kinematics(self.geometry, math_state) + + response.success = True + response.x = pose.x + response.y = pose.y + response.z = pose.z + response.phi = pose.phi_deg + response.height = self.current_state.height + response.j2 = self.current_state.j2 + response.j3 = self.current_state.j3 + response.j4 = self.current_state.j4 + response.j5 = self.current_state.j5 + response.j6 = self.current_state.j6 + + except Exception as e: + response.success = False + response.message = f'错误: {str(e)}' + self.get_logger().error(f'查询位姿失败: {e}') + + return response + + def handle_set_gripper(self, request, response): + """处理夹爪控制服务""" + try: + if request.gripper_state == SetGripper.Request.GRIPPER_OPEN: + j5 = J5_OPEN + elif request.gripper_state == SetGripper.Request.GRIPPER_CLOSED: + j5 = J5_CLOSED + else: + j5 = self.current_state.j5 + + if request.grip: + j6 = GRIP_ANGLE + elif request.release: + j6 = RELEASE_ANGLE + else: + j6 = self.current_state.j6 + + target_state = ArmJointState( + height=self.current_state.height, + j2=self.current_state.j2, + j3=self.current_state.j3, + j4=self.current_state.j4, + j5=j5, + j6=j6, + ) + + success = self.send_udp_commands([target_state], 0.0) + + response.success = success + response.message = "夹爪控制完成" if success else "夹爪控制失败" + self.get_logger().info(f'夹爪控制: j5={j5}, j6={j6} -> {response.message}') + + except Exception as e: + response.success = False + response.message = f'错误: {str(e)}' + self.get_logger().error(f'夹爪控制失败: {e}') + + return response + + def publish_state(self): + """定时发布状态""" + if self.current_state is None: + return + + try: + # 发布关节状态 + joint_msg = JointState() + joint_msg.header.stamp = self.get_clock().now().to_msg() + joint_msg.height = self.current_state.height + joint_msg.j2 = self.current_state.j2 + joint_msg.j3 = self.current_state.j3 + joint_msg.j4 = self.current_state.j4 + joint_msg.j5 = self.current_state.j5 + joint_msg.j6 = self.current_state.j6 + self.pub_joint_states.publish(joint_msg) + + # 计算并发布 TCP 位姿 + math_state = command_to_math_state(self.current_state, self.zero_offsets) + pose = forward_kinematics(self.geometry, math_state) + + pose_msg = TCPPose() + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.x = pose.x + pose_msg.y = pose.y + pose_msg.z = pose.z + pose_msg.phi = pose.phi_deg + self.pub_tcp_pose.publish(pose_msg) + + except Exception as e: + self.get_logger().error(f'发布状态失败: {e}') + + def destroy_node(self): + """节点销毁时的清理""" + self.udp_socket.close() + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = ArmControlNode() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros2/src/udp_teleop/udp_teleop/arm_control_client.py b/ros2/src/udp_teleop/udp_teleop/arm_control_client.py new file mode 100644 index 0000000..eebb4ce --- /dev/null +++ b/ros2/src/udp_teleop/udp_teleop/arm_control_client.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python3 +"""机械臂控制客户端示例 + +演示如何调用机械臂控制服务 +""" + +import rclpy +from rclpy.node import Node +from arm_control_msgs.srv import MoveJoints, MovePose, GetPose, SetGripper + + +class ArmControlClient(Node): + """机械臂控制客户端""" + + def __init__(self): + super().__init__('arm_control_client') + + # 创建服务客户端 + self.cli_move_joints = self.create_client(MoveJoints, 'arm_control/move_joints') + self.cli_move_pose = self.create_client(MovePose, 'arm_control/move_pose') + self.cli_get_pose = self.create_client(GetPose, 'arm_control/get_pose') + self.cli_set_gripper = self.create_client(SetGripper, 'arm_control/set_gripper') + + # 等待服务可用 + self.get_logger().info('等待服务...') + self.cli_move_joints.wait_for_service() + self.cli_move_pose.wait_for_service() + self.cli_get_pose.wait_for_service() + self.cli_set_gripper.wait_for_service() + self.get_logger().info('服务已连接') + + def get_current_pose(self): + """查询当前位姿""" + req = GetPose.Request() + future = self.cli_get_pose.call_async(req) + rclpy.spin_until_future_complete(self, future) + + if future.result().success: + result = future.result() + self.get_logger().info( + f'当前位姿: x={result.x:.1f}, y={result.y:.1f}, ' + f'z={result.z:.1f}, phi={result.phi:.1f}°' + ) + self.get_logger().info( + f'关节角度: height={result.height}, j2={result.j2}, ' + f'j3={result.j3}, j4={result.j4}, j5={result.j5}, j6={result.j6}' + ) + return result + else: + self.get_logger().error(f'查询失败: {future.result().message}') + return None + + def move_to_joints(self, height, j2, j3, j4, j5=81, j6=30, duration=2.0): + """关节空间运动""" + req = MoveJoints.Request() + req.height = height + req.j2 = j2 + req.j3 = j3 + req.j4 = j4 + req.j5 = j5 + req.j6 = j6 + req.duration = duration + + self.get_logger().info(f'关节运动: height={height}, j2={j2}, j3={j3}, j4={j4}') + future = self.cli_move_joints.call_async(req) + rclpy.spin_until_future_complete(self, future) + + if future.result().success: + self.get_logger().info('运动完成') + return True + else: + self.get_logger().error(f'运动失败: {future.result().message}') + return False + + def move_to_pose(self, x, y, z, phi, duration=2.0, grip=False, release=False): + """笛卡尔空间运动""" + req = MovePose.Request() + req.x = x + req.y = y + req.z = z + req.phi = phi + req.duration = duration + req.grip = grip + req.release = release + req.gripper_state = MovePose.Request.GRIPPER_KEEP + req.elbow_up = False + + self.get_logger().info(f'位姿运动: ({x:.1f}, {y:.1f}, {z:.1f}), phi={phi:.1f}°') + future = self.cli_move_pose.call_async(req) + rclpy.spin_until_future_complete(self, future) + + if future.result().success: + self.get_logger().info('运动完成') + return True + else: + self.get_logger().error(f'运动失败: {future.result().message}') + return False + + def set_gripper(self, grip=False, release=False): + """夹爪控制""" + req = SetGripper.Request() + req.grip = grip + req.release = release + req.gripper_state = SetGripper.Request.GRIPPER_KEEP + + action = "抓取" if grip else ("释放" if release else "保持") + self.get_logger().info(f'夹爪控制: {action}') + future = self.cli_set_gripper.call_async(req) + rclpy.spin_until_future_complete(self, future) + + if future.result().success: + self.get_logger().info('夹爪控制完成') + return True + else: + self.get_logger().error(f'夹爪控制失败: {future.result().message}') + return False + + +def demo_sequence(client): + """演示抓取流程""" + print("\n" + "="*60) + print("演示序列：查询 → 移动 → 抓取") + print("="*60 + "\n") + + # 1. 查询当前位姿 + print("1. 查询当前位姿...") + client.get_current_pose() + + # 2. 移动到上方 + print("\n2. 移动到物体上方...") + client.move_to_pose(x=200.0, y=100.0, z=-50.0, phi=45.0, duration=2.0, release=True) + + # 3. 下降到抓取位置 + print("\n3. 下降到抓取位置...") + client.move_to_pose(x=200.0, y=100.0, z=-150.0, phi=45.0, duration=1.0, release=True) + + # 4. 抓取 + print("\n4. 执行抓取...") + client.set_gripper(grip=True) + + # 5. 提升 + print("\n5. 提升物体...") + client.move_to_pose(x=200.0, y=100.0, z=-50.0, phi=45.0, duration=1.0, grip=True) + + # 6. 移动到目标位置 + print("\n6. 移动到目标位置...") + client.move_to_pose(x=100.0, y=200.0, z=-50.0, phi=90.0, duration=2.0, grip=True) + + # 7. 下降 + print("\n7. 下降...") + client.move_to_pose(x=100.0, y=200.0, z=-150.0, phi=90.0, duration=1.0, grip=True) + + # 8. 释放 + print("\n8. 释放物体...") + client.set_gripper(release=True) + + # 9. 提升 + print("\n9. 提升...") + client.move_to_pose(x=100.0, y=200.0, z=-50.0, phi=90.0, duration=1.0, release=True) + + print("\n" + "="*60) + print("演示完成！") + print("="*60 + "\n") + + +def main(): + rclpy.init() + client = ArmControlClient() + + try: + # 运行演示序列 + demo_sequence(client) + + except KeyboardInterrupt: + pass + finally: + client.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros2/src/udp_teleop/udp_teleop/vision_grasp.py b/ros2/src/udp_teleop/udp_teleop/vision_grasp.py new file mode 100644 index 0000000..b6c72ca --- /dev/null +++ b/ros2/src/udp_teleop/udp_teleop/vision_grasp.py @@ -0,0 +1,428 @@ +#!/usr/bin/env python3 +"""视觉抓取 ROS 节点 + +基于相机坐标系到基坐标系的变换，实现自动抓取和释放功能。 + +功能： +1. 抓取服务：输入相机坐标 (x, y, z)，自动转换并执行抓取 +2. 释放服务：输入基坐标系位置，移动并释放物体 +""" + +import math +from typing import Tuple + +import numpy as np +import rclpy +from rclpy.node import Node + +from arm_control_msgs.srv import MovePose, GetPose, SetGripper +from std_srvs.srv import Trigger +from geometry_msgs.msg import Point + + +# ============================================================================ +# 坐标变换函数（从 camera_to_base.py 复制） +# ============================================================================ + +def euler_to_rotation_matrix(roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray: + """欧拉角转旋转矩阵（ZYX顺序）""" + roll = math.radians(roll_deg) + pitch = math.radians(pitch_deg) + yaw = math.radians(yaw_deg) + + Rx = np.array([ + [1, 0, 0], + [0, math.cos(roll), -math.sin(roll)], + [0, math.sin(roll), math.cos(roll)] + ]) + + Ry = np.array([ + [math.cos(pitch), 0, math.sin(pitch)], + [0, 1, 0], + [-math.sin(pitch), 0, math.cos(pitch)] + ]) + + Rz = np.array([ + [math.cos(yaw), -math.sin(yaw), 0], + [math.sin(yaw), math.cos(yaw), 0], + [0, 0, 1] + ]) + + return Rz @ Ry @ Rx + + +def camera_to_tcp( + xc: float, yc: float, zc: float, + tx: float = 0.0, ty: float = 0.0, tz: float = 0.0, + roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0 +) -> Tuple[float, float, float]: + """相机坐标系 → TCP 坐标系""" + R = euler_to_rotation_matrix(roll, pitch, yaw) + T = np.array([tx, ty, tz]) + P_cam = np.array([xc, yc, zc]) + P_tcp = R @ P_cam + T + return float(P_tcp[0]), float(P_tcp[1]), float(P_tcp[2]) + + +def tcp_to_base( + xt: float, yt: float, zt: float, + tcp_x: float, tcp_y: float, tcp_z: float, tcp_phi_deg: float +) -> Tuple[float, float, float]: + """TCP 坐标系 → 机械臂基坐标系（水平相机版本）""" + phi = math.radians(tcp_phi_deg) + + R_tcp_to_base = np.array([ + [-math.sin(phi), 0, math.cos(phi)], + [ math.cos(phi), 0, math.sin(phi)], + [0, -1, 0] + ]) + + P_tcp = np.array([xt, yt, zt]) + P_base_relative = R_tcp_to_base @ P_tcp + P_base = P_base_relative + np.array([tcp_x, tcp_y, tcp_z]) + + return float(P_base[0]), float(P_base[1]), float(P_base[2]) + + +def camera_to_base( + xc: float, yc: float, zc: float, + tcp_x: float, tcp_y: float, tcp_z: float, tcp_phi_deg: float, + cam_tx: float = 0.0, cam_ty: float = 0.0, cam_tz: float = 0.0, + cam_roll: float = 0.0, cam_pitch: float = 0.0, cam_yaw: float = 0.0 +) -> Tuple[float, float, float]: + """完整变换：相机坐标系 → 基坐标系""" + xt, yt, zt = camera_to_tcp(xc, yc, zc, cam_tx, cam_ty, cam_tz, + cam_roll, cam_pitch, cam_yaw) + xb, yb, zb = tcp_to_base(xt, yt, zt, tcp_x, tcp_y, tcp_z, tcp_phi_deg) + return xb, yb, zb + + +# ============================================================================ +# 自定义服务定义 +# ============================================================================ + +# 由于没有预定义服务，我们使用简化的接口 +# 实际使用时可以创建自定义 .srv 文件 + + +# ============================================================================ +# 视觉抓取节点 +# ============================================================================ + +class VisionGraspNode(Node): + """视觉抓取 ROS 节点""" + + def __init__(self): + super().__init__('vision_grasp') + + # 声明参数 + self.declare_parameters( + namespace='', + parameters=[ + # 相机到 TCP 的变换参数 + ('cam_tx', 0.0), + ('cam_ty', 0.0), + ('cam_tz', 0.0), + ('cam_roll', 0.0), + ('cam_pitch', 0.0), + ('cam_yaw', 0.0), + # 抓取参数 + ('approach_height_offset', 50.0), # 接近高度偏移 (mm) + ('retract_position_x', 200.0), # 回收位置 X + ('retract_position_y', 0.0), # 回收位置 Y + ('grasp_duration', 3.0), # 抓取移动时长 (秒) + ('release_duration', 2.0), # 释放移动时长 (秒) + ('gripper_duration', 1.0), # 夹爪动作时长 (秒) + ] + ) + + # 获取参数 + self.cam_tx = self.get_parameter('cam_tx').value + self.cam_ty = self.get_parameter('cam_ty').value + self.cam_tz = self.get_parameter('cam_tz').value + self.cam_roll = self.get_parameter('cam_roll').value + self.cam_pitch = self.get_parameter('cam_pitch').value + self.cam_yaw = self.get_parameter('cam_yaw').value + + self.approach_offset = self.get_parameter('approach_height_offset').value + self.retract_x = self.get_parameter('retract_position_x').value + self.retract_y = self.get_parameter('retract_position_y').value + self.grasp_duration = self.get_parameter('grasp_duration').value + self.release_duration = self.get_parameter('release_duration').value + self.gripper_duration = self.get_parameter('gripper_duration').value + + # 创建服务客户端（连接到 arm_control 节点） + self.move_cli = self.create_client(MovePose, 'arm_control/move_pose') + self.get_pose_cli = self.create_client(GetPose, 'arm_control/get_pose') + self.gripper_cli = self.create_client(SetGripper, 'arm_control/set_gripper') + + # 等待服务可用 + self.get_logger().info('等待 arm_control 服务...') + self.move_cli.wait_for_service(timeout_sec=5.0) + self.get_pose_cli.wait_for_service(timeout_sec=5.0) + self.gripper_cli.wait_for_service(timeout_sec=5.0) + self.get_logger().info('arm_control 服务已连接') + + # 创建订阅者：接收检测结果 + self.grasp_sub = self.create_subscription( + Point, + 'vision_grasp/grasp_target', + self.handle_grasp_target, + 10 + ) + + self.release_sub = self.create_subscription( + Point, + 'vision_grasp/release_target', + self.handle_release_target, + 10 + ) + + self.get_logger().info('视觉抓取节点已启动') + self.get_logger().info('订阅话题:') + self.get_logger().info(' - /vision_grasp/grasp_target (geometry_msgs/Point)') + self.get_logger().info(' - /vision_grasp/release_target (geometry_msgs/Point)') + + def get_current_tcp_pose(self) -> Tuple[float, float, float, float]: + """查询当前 TCP 位姿""" + req = GetPose.Request() + future = self.get_pose_cli.call_async(req) + + # 等待结果（不阻塞其他回调） + import time + start_time = time.time() + timeout = 5.0 + + while not future.done(): + if time.time() - start_time > timeout: + raise RuntimeError("获取 TCP 位姿超时") + time.sleep(0.01) + + result = future.result() + if not result.success: + raise RuntimeError(f"获取 TCP 位姿失败: {result.message}") + + return result.x, result.y, result.z, result.phi + + def move_to(self, x: float, y: float, z: float, phi: float, + duration: float, grip: bool = False, release: bool = False) -> bool: + """移动到指定位置""" + req = MovePose.Request() + req.x = x + req.y = y + req.z = z + req.phi = phi + req.duration = duration + req.grip = grip + req.release = release + req.gripper_state = MovePose.Request.GRIPPER_KEEP + req.elbow_up = False + + future = self.move_cli.call_async(req) + + # 等待结果 + import time + start_time = time.time() + timeout = duration + 5.0 + + while not future.done(): + if time.time() - start_time > timeout: + self.get_logger().error("移动超时") + return False + time.sleep(0.01) + + result = future.result() + if result is None: + self.get_logger().error("移动失败：无响应") + return False + + return result.success + + def set_gripper(self, grip: bool = False, release: bool = False) -> bool: + """控制夹爪""" + req = SetGripper.Request() + req.grip = grip + req.release = release + req.gripper_state = SetGripper.Request.GRIPPER_KEEP + + future = self.gripper_cli.call_async(req) + + # 等待结果 + import time + start_time = time.time() + timeout = 3.0 + + while not future.done(): + if time.time() - start_time > timeout: + self.get_logger().error("夹爪控制超时") + return False + time.sleep(0.01) + + result = future.result() + if result is None: + self.get_logger().error("夹爪控制失败：无响应") + return False + + return result.success + + def handle_grasp_target(self, msg: Point): + """处理抓取目标""" + self.get_logger().info(f'收到抓取目标: 相机坐标 ({msg.x:.1f}, {msg.y:.1f}, {msg.z:.1f})') + + # 在单独的线程中执行抓取，避免阻塞回调 + import threading + thread = threading.Thread( + target=self._execute_grasp_thread, + args=(msg.x, msg.y, msg.z) + ) + thread.start() + + def _execute_grasp_thread(self, x: float, y: float, z: float): + """在独立线程中执行抓取流程""" + try: + # 转换坐标：(x, y, z) -> (xc, yc, zc) = (x, -y, z) + xc = x + yc = -y + zc = z + + self.get_logger().info(f'转换后相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})') + + # 获取当前 TCP 位姿 + tcp_x, tcp_y, tcp_z, tcp_phi = self.get_current_tcp_pose() + self.get_logger().info(f'当前 TCP: ({tcp_x:.1f}, {tcp_y:.1f}, {tcp_z:.1f}), phi={tcp_phi:.1f}°') + + # 坐标变换 + target_x, target_y, target_z = camera_to_base( + xc, yc, zc, + tcp_x, tcp_y, tcp_z, tcp_phi, + self.cam_tx, self.cam_ty, self.cam_tz, + self.cam_roll, self.cam_pitch, self.cam_yaw + ) + + self.get_logger().info(f'目标基坐标: ({target_x:.1f}, {target_y:.1f}, {target_z:.1f})') + + # 执行抓取流程 + self.execute_grasp(target_x, target_y, target_z, tcp_phi) + + except Exception as e: + self.get_logger().error(f'抓取失败: {e}') + + def handle_release_target(self, msg: Point): + """处理释放目标""" + self.get_logger().info(f'收到释放目标: 基坐标 ({msg.x:.1f}, {msg.y:.1f}, {msg.z:.1f})') + + # 在单独的线程中执行释放，避免阻塞回调 + import threading + thread = threading.Thread( + target=self._execute_release_thread, + args=(msg.x, msg.y, msg.z) + ) + thread.start() + + def _execute_release_thread(self, x: float, y: float, z: float): + """在独立线程中执行释放流程""" + try: + # 获取当前 TCP 位姿（用于获取 phi） + _, _, _, tcp_phi = self.get_current_tcp_pose() + + # 执行释放流程 + self.execute_release(x, y, z, tcp_phi) + + except Exception as e: + self.get_logger().error(f'释放失败: {e}') + + def execute_grasp(self, x: float, y: float, z: float, phi: float): + """执行抓取流程 + + 1. release 夹爪 (duration=0) + 2. 移动到目标位置 (duration=3) + 3. grip 夹爪 (duration=1) + 4. 移动到回收位置 (200, 0, 当前z) + """ + self.get_logger().info('=' * 60) + self.get_logger().info('开始抓取流程') + self.get_logger().info('=' * 60) + + # 步骤 1: 释放夹爪 + self.get_logger().info('1. 释放夹爪') + if not self.set_gripper(release=True): + self.get_logger().error('释放夹爪失败') + return + + # 步骤 2: 移动到目标位置 + self.get_logger().info(f'2. 移动到目标位置: ({x:.1f}, {y:.1f}, {z:.1f})') + if not self.move_to(x, y, z, phi, self.grasp_duration, release=True): + self.get_logger().error('移动到目标位置失败') + return + + # 步骤 3: 抓取 + self.get_logger().info('3. 抓取物体') + if not self.move_to(x, y, z, phi, self.gripper_duration, grip=True): + self.get_logger().error('抓取失败') + return + + # 步骤 4: 移动到回收位置 + self.get_logger().info(f'4. 移动到回收位置: ({self.retract_x:.1f}, {self.retract_y:.1f}, {z:.1f})') + if not self.move_to(self.retract_x, self.retract_y, z, phi, self.grasp_duration, grip=True): + self.get_logger().error('移动到回收位置失败') + return + + self.get_logger().info('=' * 60) + self.get_logger().info('✓ 抓取完成！') + self.get_logger().info('=' * 60) + + def execute_release(self, x: float, y: float, z: float, phi: float): + """执行释放流程 + + 1. 移动到指定位置 + 2. release 夹爪 (duration=0) + 3. 回收到 (200, 0, 当前z) + """ + self.get_logger().info('=' * 60) + self.get_logger().info('开始释放流程') + self.get_logger().info('=' * 60) + + # 步骤 1: 移动到释放位置 + self.get_logger().info(f'1. 移动到释放位置: ({x:.1f}, {y:.1f}, {z:.1f})') + if not self.move_to(x, y, z, phi, self.release_duration, grip=True): + self.get_logger().error('移动到释放位置失败') + return + + # 步骤 2: 释放夹爪 + self.get_logger().info('2. 释放物体') + if not self.set_gripper(release=True): + self.get_logger().error('释放夹爪失败') + return + + # 步骤 3: 回收到初始位置 + self.get_logger().info(f'3. 回收到初始位置: ({self.retract_x:.1f}, {self.retract_y:.1f}, {z:.1f})') + if not self.move_to(self.retract_x, self.retract_y, z, phi, self.release_duration, release=True): + self.get_logger().error('回收失败') + return + + self.get_logger().info('=' * 60) + self.get_logger().info('✓ 释放完成！') + self.get_logger().info('=' * 60) + + +def main(args=None): + rclpy.init(args=args) + + # 使用多线程执行器以支持在回调中调用服务 + from rclpy.executors import MultiThreadedExecutor + + node = VisionGraspNode() + executor = MultiThreadedExecutor() + executor.add_node(node) + + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main()