mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2026-02-05 22:15:04 +00:00
fix:debug dummy2
This commit is contained in:
ZiWei
325
dummy2_debug/test_dummy2_deep.py
Normal file
325
dummy2_debug/test_dummy2_deep.py
Normal file
@@ -0,0 +1,325 @@
|
||||
#!/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()
|
||||
Reference in New Issue
Block a user