mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2026-02-05 05:45:10 +00:00
326 lines
10 KiB
Python
326 lines
10 KiB
Python
#!/usr/bin/env python3
|
||
"""
|
||
Dummy2 Unilab实际控制功能测试
|
||
测试通过Unilab系统控制Dummy2机械臂的功能
|
||
"""
|
||
|
||
import json
|
||
import time
|
||
import sys
|
||
import os
|
||
import threading
|
||
|
||
# 添加Unilab路径
|
||
sys.path.insert(0, '/home/hh/Uni-Lab-OS')
|
||
|
||
def test_ros2_node_creation():
|
||
"""测试ROS2节点创建"""
|
||
print("=" * 50)
|
||
print("测试1: ROS2节点创建和初始化")
|
||
print("=" * 50)
|
||
|
||
try:
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
|
||
# 初始化ROS2
|
||
rclpy.init()
|
||
print("✓ ROS2系统初始化成功")
|
||
|
||
# 创建简单的测试节点（不使用BaseROS2DeviceNode，因为它需要太多参数）
|
||
test_node = Node("test_dummy2_node")
|
||
test_node.device_id = "test_dummy2"
|
||
# 添加callback_group属性
|
||
test_node.callback_group = rclpy.callback_groups.ReentrantCallbackGroup()
|
||
print("✓ 测试节点创建成功")
|
||
|
||
# 启动executor
|
||
executor = rclpy.executors.MultiThreadedExecutor()
|
||
executor.add_node(test_node)
|
||
|
||
# 在后台线程中运行executor
|
||
executor_thread = threading.Thread(target=executor.spin, daemon=True)
|
||
executor_thread.start()
|
||
print("✓ ROS2 executor启动成功")
|
||
|
||
# 等待节点初始化
|
||
time.sleep(2)
|
||
|
||
return test_node, executor, executor_thread
|
||
|
||
except Exception as e:
|
||
print(f"✗ ROS2节点创建失败: {e}")
|
||
import traceback
|
||
traceback.print_exc()
|
||
return None, None, None
|
||
|
||
def test_moveit_interface_with_ros2(test_node):
|
||
"""测试MoveitInterface与ROS2节点的集成"""
|
||
print("\n" + "=" * 50)
|
||
print("测试2: MoveitInterface与ROS2集成")
|
||
print("=" * 50)
|
||
|
||
try:
|
||
from unilabos.devices.ros_dev.moveit_interface import MoveitInterface
|
||
|
||
# 创建MoveitInterface实例
|
||
moveit_interface = MoveitInterface(
|
||
moveit_type='dummy2_robot',
|
||
joint_poses='[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]',
|
||
device_config=None
|
||
)
|
||
print("✓ MoveitInterface实例创建成功")
|
||
|
||
# 执行post_init
|
||
moveit_interface.post_init(test_node)
|
||
print("✓ post_init执行成功")
|
||
|
||
# 检查moveit2实例是否创建
|
||
if hasattr(moveit_interface, 'moveit2') and moveit_interface.moveit2:
|
||
print(f"✓ MoveIt2实例创建成功，可用组: {list(moveit_interface.moveit2.keys())}")
|
||
else:
|
||
print("✗ MoveIt2实例创建失败")
|
||
|
||
return moveit_interface
|
||
|
||
except Exception as e:
|
||
print(f"✗ MoveitInterface集成失败: {e}")
|
||
import traceback
|
||
traceback.print_exc()
|
||
return None
|
||
|
||
def test_joint_position_validation():
|
||
"""测试关节位置验证"""
|
||
print("\n" + "=" * 50)
|
||
print("测试3: 关节位置参数验证")
|
||
print("=" * 50)
|
||
|
||
try:
|
||
# 测试不同的关节位置格式
|
||
test_positions = [
|
||
"[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]", # 字符串格式
|
||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], # 列表格式
|
||
[1.0, 0.5, -0.5, 0.0, 1.0, 0.0], # 测试位置
|
||
]
|
||
|
||
for i, pos in enumerate(test_positions, 1):
|
||
try:
|
||
if isinstance(pos, str):
|
||
parsed_pos = json.loads(pos)
|
||
else:
|
||
parsed_pos = pos
|
||
|
||
if len(parsed_pos) == 6:
|
||
print(f"✓ 位置{i}格式正确: {parsed_pos}")
|
||
else:
|
||
print(f"✗ 位置{i}关节数量错误: {len(parsed_pos)}")
|
||
|
||
except Exception as e:
|
||
print(f"✗ 位置{i}解析失败: {e}")
|
||
|
||
except Exception as e:
|
||
print(f"✗ 关节位置验证失败: {e}")
|
||
|
||
def test_action_command_format():
|
||
"""测试Action命令格式"""
|
||
print("\n" + "=" * 50)
|
||
print("测试4: Action命令格式验证")
|
||
print("=" * 50)
|
||
|
||
try:
|
||
# 测试moveit_joint_task命令格式
|
||
joint_task_cmd = {
|
||
"move_group": "arm",
|
||
"joint_positions": "[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]",
|
||
"speed": 0.3,
|
||
"retry": 10
|
||
}
|
||
|
||
print("关节空间任务命令:")
|
||
print(f" {json.dumps(joint_task_cmd, indent=2)}")
|
||
print("✓ 关节空间命令格式正确")
|
||
|
||
# 测试moveit_task命令格式
|
||
cartesian_task_cmd = {
|
||
"move_group": "arm",
|
||
"position": [0.3, 0.0, 0.4],
|
||
"quaternion": [0.0, 0.0, 0.0, 1.0],
|
||
"speed": 0.3,
|
||
"retry": 10,
|
||
"cartesian": False
|
||
}
|
||
|
||
print("\n笛卡尔空间任务命令:")
|
||
print(f" {json.dumps(cartesian_task_cmd, indent=2)}")
|
||
print("✓ 笛卡尔空间命令格式正确")
|
||
|
||
except Exception as e:
|
||
print(f"✗ 命令格式验证失败: {e}")
|
||
|
||
def test_joint_name_mapping():
|
||
"""测试关节名称映射"""
|
||
print("\n" + "=" * 50)
|
||
print("测试5: 关节名称映射验证")
|
||
print("=" * 50)
|
||
|
||
try:
|
||
# Unilab配置中的关节名称
|
||
unilab_joints = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
|
||
|
||
# ROS2 dummy2_ws中的关节名称
|
||
ros2_joints = ['Joint1', 'Joint2', 'Joint3', 'Joint4', 'Joint5', 'Joint6']
|
||
|
||
print("关节名称映射:")
|
||
print("Unilab配置 -> ROS2配置")
|
||
for unilab, ros2 in zip(unilab_joints, ros2_joints):
|
||
print(f" {unilab} -> {ros2}")
|
||
|
||
print("\n注意: 可能需要在MoveitInterface中处理关节名称映射")
|
||
print("✓ 关节名称映射检查完成")
|
||
|
||
except Exception as e:
|
||
print(f"✗ 关节名称映射检查失败: {e}")
|
||
|
||
def test_device_id_prefix():
|
||
"""测试设备ID前缀"""
|
||
print("\n" + "=" * 50)
|
||
print("测试6: 设备ID前缀处理")
|
||
print("=" * 50)
|
||
|
||
try:
|
||
# 模拟设备ID前缀处理
|
||
device_id = "dummy2_01"
|
||
base_joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
|
||
|
||
# 添加设备ID前缀
|
||
prefixed_joints = [f"{device_id}_{name}" for name in base_joint_names]
|
||
|
||
print(f"设备ID: {device_id}")
|
||
print("带前缀的关节名称:")
|
||
for joint in prefixed_joints:
|
||
print(f" {joint}")
|
||
|
||
# 同样处理link名称
|
||
base_link = f"{device_id}_base_link"
|
||
end_effector = f"{device_id}_tool_link"
|
||
|
||
print(f"\n基础连接: {base_link}")
|
||
print(f"末端执行器: {end_effector}")
|
||
print("✓ 设备ID前缀处理正确")
|
||
|
||
except Exception as e:
|
||
print(f"✗ 设备ID前缀处理失败: {e}")
|
||
|
||
def test_moveit_interface_methods(moveit_interface):
|
||
"""测试MoveitInterface方法调用"""
|
||
print("\n" + "=" * 50)
|
||
print("测试7: MoveitInterface方法测试")
|
||
print("=" * 50)
|
||
|
||
if moveit_interface is None:
|
||
print("✗ MoveitInterface实例不可用，跳过方法测试")
|
||
return
|
||
|
||
try:
|
||
# 测试moveit_joint_task方法
|
||
print("测试moveit_joint_task方法...")
|
||
test_joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
|
||
# 注意：这里不实际执行，只测试方法调用格式
|
||
print(f" 测试参数: move_group='arm', joint_positions={test_joint_positions}")
|
||
print("✓ moveit_joint_task方法可调用")
|
||
|
||
# 测试moveit_task方法
|
||
print("\n测试moveit_task方法...")
|
||
test_position = [0.3, 0.0, 0.4]
|
||
test_quaternion = [0.0, 0.0, 0.0, 1.0]
|
||
|
||
print(f" 测试参数: move_group='arm', position={test_position}, quaternion={test_quaternion}")
|
||
print("✓ moveit_task方法可调用")
|
||
|
||
except Exception as e:
|
||
print(f"✗ 方法测试失败: {e}")
|
||
|
||
def cleanup_ros2(executor, executor_thread):
|
||
"""清理ROS2资源"""
|
||
print("\n" + "=" * 50)
|
||
print("清理ROS2资源")
|
||
print("=" * 50)
|
||
|
||
try:
|
||
import rclpy
|
||
import signal
|
||
import os
|
||
|
||
# 设置超时处理
|
||
def timeout_handler(signum, frame):
|
||
print("✗ 清理超时，强制退出")
|
||
os._exit(0)
|
||
|
||
signal.signal(signal.SIGALRM, timeout_handler)
|
||
signal.alarm(5) # 5秒超时
|
||
|
||
if executor:
|
||
try:
|
||
executor.shutdown()
|
||
print("✓ Executor已关闭")
|
||
except Exception as e:
|
||
print(f"✗ Executor关闭失败: {e}")
|
||
|
||
if executor_thread and executor_thread.is_alive():
|
||
try:
|
||
executor_thread.join(timeout=2)
|
||
if executor_thread.is_alive():
|
||
print("✗ Executor线程未能正常结束")
|
||
else:
|
||
print("✓ Executor线程已结束")
|
||
except Exception as e:
|
||
print(f"✗ 线程结束失败: {e}")
|
||
|
||
try:
|
||
rclpy.shutdown()
|
||
print("✓ ROS2系统已关闭")
|
||
except Exception as e:
|
||
print(f"✗ ROS2关闭失败: {e}")
|
||
|
||
signal.alarm(0) # 取消超时
|
||
|
||
except Exception as e:
|
||
print(f"✗ 清理过程中出错: {e}")
|
||
# 强制退出
|
||
import os
|
||
os._exit(0)
|
||
|
||
def main():
|
||
"""主测试函数"""
|
||
print("Dummy2 Unilab控制功能深度测试")
|
||
print("=" * 60)
|
||
|
||
# 测试基础功能
|
||
test_joint_position_validation()
|
||
test_action_command_format()
|
||
test_joint_name_mapping()
|
||
test_device_id_prefix()
|
||
|
||
# 测试ROS2集成
|
||
test_node, executor, executor_thread = test_ros2_node_creation()
|
||
|
||
if test_node:
|
||
moveit_interface = test_moveit_interface_with_ros2(test_node)
|
||
test_moveit_interface_methods(moveit_interface)
|
||
|
||
# 等待一段时间观察系统状态
|
||
print("\n等待3秒观察系统状态...")
|
||
time.sleep(3)
|
||
|
||
cleanup_ros2(executor, executor_thread)
|
||
else:
|
||
print("ROS2节点创建失败，跳过集成测试")
|
||
|
||
print("\n" + "=" * 60)
|
||
print("深度测试完成")
|
||
print("=" * 60)
|
||
|
||
if __name__ == "__main__":
|
||
main()
|