fallensigh/craic
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 添加机械臂 ROS 2 控制节点和视觉抓取系统

Browse Source 
- 创建 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
This commit is contained in:
FallenSigh
2026-06-16 18:45:01 +08:00
parent 7144005be2
commit 83b32542ef
22 changed files with 3119 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
ros2/src/arm_control_msgs/CMakeLists.txt Normal file
View File

@@ -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()

8
ros2/src/arm_control_msgs/msg/JointState.msg Normal file
View File

@@ -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 角度 (度)

6
ros2/src/arm_control_msgs/msg/TCPPose.msg Normal file
View File

@@ -0,0 +1,6 @@
# TCP 位姿消息
std_msgs/Header header
float64 x # X 坐标 (mm)
float64 y # Y 坐标 (mm)
float64 z # Z 坐标 (mm)
float64 phi # 偏航角 (度)

22
ros2/src/arm_control_msgs/package.xml Normal file
View File

@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>arm_control_msgs</name>
<version>0.0.1</version>
<description>Message and service definitions for arm control</description>
<maintainer email="fallensigh@gmail.com">fallensigh</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>std_msgs</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

14
ros2/src/arm_control_msgs/srv/GetPose.srv Normal file
View File

@@ -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 角度 (度)

11
ros2/src/arm_control_msgs/srv/MoveJoints.srv Normal file
View File

@@ -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 # 返回信息

18
ros2/src/arm_control_msgs/srv/MovePose.srv Normal file
View File

@@ -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 # 返回信息

12
ros2/src/arm_control_msgs/srv/SetGripper.srv Normal file
View File

@@ -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 # 返回信息

38
ros2/src/udp_teleop/config/arm_control.yaml Normal file
View File

@@ -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

21
ros2/src/udp_teleop/config/vision_grasp.yaml Normal file
View File

@@ -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 # 夹爪动作时长 (秒)

8
ros2/src/udp_teleop/package.xml
View File

@@ -3,9 +3,13 @@
<package format="3">
<name>udp_teleop</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<description>UDP teleoperation and arm control for CRAIC robot</description>
<maintainer email="fallensigh@gmail.com">fallensigh</maintainer>
<license>TODO: License declaration</license>
<license>MIT</license>
<depend>rclpy</depend>
<depend>std_msgs</depend>
<depend>arm_control_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>

4
ros2/src/udp_teleop/setup.py
View File

@@ -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'
],
},
)

743
ros2/src/udp_teleop/udp_teleop/arm_control.py Normal file
View File

@@ -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()

182
ros2/src/udp_teleop/udp_teleop/arm_control_client.py Normal file
View File

@@ -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()

428
ros2/src/udp_teleop/udp_teleop/vision_grasp.py Normal file
View File

@@ -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()