fix:debug dummy2

dummy2_debug/debug_dummy2_integration.py

@@ -0,0 +1,219 @@
+#!/usr/bin/env python3
+"""
+Dummy2 Unilab集成调试脚本
+用于测试Dummy2机械臂在Unilab系统中的控制功能
+"""
+
+import json
+import time
+import sys
+import os
+
+# 添加Unilab路径
+sys.path.insert(0, '/home/hh/Uni-Lab-OS')
+
+def test_device_registration():
+    """测试设备注册配置"""
+    print("=" * 50)
+    print("测试1: 设备注册配置")
+    print("=" * 50)
+    
+    try:
+        import yaml
+        with open('/home/hh/Uni-Lab-OS/unilabos/registry/devices/robot_arm.yaml', 'r', encoding='utf-8') as f:
+            config = yaml.safe_load(f)
+        
+        if 'robotic_arm.Dummy2' in config:
+            print("✓ Dummy2设备已注册")
+            
+            # 检查关键配置
+            dummy2_config = config['robotic_arm.Dummy2']
+            
+            # 检查模块配置
+            if 'class' in dummy2_config and 'module' in dummy2_config['class']:
+                module_path = dummy2_config['class']['module']
+                print(f"✓ 模块路径: {module_path}")
+                
+                # 检查action配置
+                if 'action_value_mappings' in dummy2_config['class']:
+                    actions = dummy2_config['class']['action_value_mappings']
+                    print(f"✓ 可用actions: {list(actions.keys())}")
+                else:
+                    print("✗ 未找到action配置")
+            else:
+                print("✗ 未找到模块配置")
+        else:
+            print("✗ Dummy2设备未注册")
+            
+    except Exception as e:
+        print(f"✗ 配置文件读取错误: {e}")
+
+def test_device_mesh_config():
+    """测试设备网格配置"""
+    print("\n" + "=" * 50)
+    print("测试2: 设备网格配置")
+    print("=" * 50)
+    
+    try:
+        # 检查move_group.json
+        config_path = '/home/hh/Uni-Lab-OS/unilabos/device_mesh/devices/dummy2_robot/config/move_group.json'
+        if os.path.exists(config_path):
+            with open(config_path, 'r') as f:
+                move_group_config = json.load(f)
+            print("✓ move_group.json配置存在")
+            print(f"  关节组: {list(move_group_config.keys())}")
+            
+            for group, config in move_group_config.items():
+                print(f"  {group}组配置:")
+                print(f"    关节名称: {config.get('joint_names', [])}")
+                print(f"    基础连接: {config.get('base_link_name', 'N/A')}")
+                print(f"    末端执行器: {config.get('end_effector_name', 'N/A')}")
+        else:
+            print("✗ move_group.json配置文件不存在")
+            
+        # 检查XACRO文件
+        xacro_path = '/home/hh/Uni-Lab-OS/unilabos/device_mesh/devices/dummy2_robot/meshes/dummy2.xacro'
+        if os.path.exists(xacro_path):
+            print("✓ dummy2.xacro模型文件存在")
+        else:
+            print("✗ dummy2.xacro模型文件不存在")
+            
+    except Exception as e:
+        print(f"✗ 设备网格配置检查错误: {e}")
+
+def test_moveit_interface_import():
+    """测试MoveitInterface模块导入"""
+    print("\n" + "=" * 50)
+    print("测试3: MoveitInterface模块导入")
+    print("=" * 50)
+    
+    try:
+        from unilabos.devices.ros_dev.moveit_interface import MoveitInterface
+        print("✓ MoveitInterface模块导入成功")
+        
+        # 检查必要的方法
+        methods = ['post_init', 'moveit_task', 'moveit_joint_task']
+        for method in methods:
+            if hasattr(MoveitInterface, method):
+                print(f"✓ 方法 {method} 存在")
+            else:
+                print(f"✗ 方法 {method} 不存在")
+                
+    except ImportError as e:
+        print(f"✗ MoveitInterface模块导入失败: {e}")
+    except Exception as e:
+        print(f"✗ 模块检查错误: {e}")
+
+def test_ros2_dependencies():
+    """测试ROS2依赖"""
+    print("\n" + "=" * 50)
+    print("测试4: ROS2依赖检查")
+    print("=" * 50)
+    
+    try:
+        import rclpy
+        print("✓ rclpy导入成功")
+        
+        from moveit_msgs.msg import JointConstraint, Constraints
+        print("✓ moveit_msgs导入成功")
+        
+        from unilabos_msgs.action import SendCmd
+        print("✓ unilabos_msgs导入成功")
+        
+        from tf2_ros import Buffer, TransformListener
+        print("✓ tf2_ros导入成功")
+        
+    except ImportError as e:
+        print(f"✗ ROS2依赖导入失败: {e}")
+
+def test_dummy2_configuration():
+    """测试Dummy2配置参数"""
+    print("\n" + "=" * 50)
+    print("测试5: Dummy2配置参数验证")
+    print("=" * 50)
+    
+    try:
+        # 模拟MoveitInterface初始化参数
+        test_params = {
+            'moveit_type': 'dummy2_robot',
+            'joint_poses': '[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]',
+            'device_config': None
+        }
+        
+        print("测试参数:")
+        for key, value in test_params.items():
+            print(f"  {key}: {value}")
+            
+        # 检查config文件是否可以被正确加载
+        config_path = f"/home/hh/Uni-Lab-OS/unilabos/device_mesh/devices/{test_params['moveit_type']}/config/move_group.json"
+        if os.path.exists(config_path):
+            with open(config_path, 'r') as f:
+                config_data = json.load(f)
+            print(f"✓ 配置文件可正常加载: {list(config_data.keys())}")
+        else:
+            print(f"✗ 配置文件不存在: {config_path}")
+            
+    except Exception as e:
+        print(f"✗ 配置参数验证错误: {e}")
+
+def test_create_dummy2_instance():
+    """测试创建Dummy2实例"""
+    print("\n" + "=" * 50)
+    print("测试6: 创建Dummy2实例")
+    print("=" * 50)
+    
+    try:
+        from unilabos.devices.ros_dev.moveit_interface import MoveitInterface
+        
+        # 创建MoveitInterface实例
+        dummy2_interface = MoveitInterface(
+            moveit_type='dummy2_robot',
+            joint_poses='[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]',
+            device_config=None
+        )
+        
+        print("✓ Dummy2 MoveitInterface实例创建成功")
+        print(f"  数据配置: {dummy2_interface.data_config}")
+        print(f"  关节位置: {dummy2_interface.joint_poses}")
+        
+    except Exception as e:
+        print(f"✗ Dummy2实例创建失败: {e}")
+
+def check_ros2_environment():
+    """检查ROS2环境"""
+    print("\n" + "=" * 50)
+    print("测试7: ROS2环境检查")
+    print("=" * 50)
+    
+    ros_distro = os.environ.get('ROS_DISTRO')
+    if ros_distro:
+        print(f"✓ ROS2版本: {ros_distro}")
+    else:
+        print("✗ ROS_DISTRO环境变量未设置")
+        
+    ament_prefix_path = os.environ.get('AMENT_PREFIX_PATH')
+    if ament_prefix_path:
+        print("✓ AMENT_PREFIX_PATH已设置")
+    else:
+        print("✗ AMENT_PREFIX_PATH环境变量未设置")
+
+def main():
+    """主测试函数"""
+    print("Dummy2 Unilab集成调试测试")
+    print("=" * 60)
+    
+    # 运行所有测试
+    test_device_registration()
+    test_device_mesh_config()
+    test_moveit_interface_import()
+    test_ros2_dependencies()
+    test_dummy2_configuration()
+    test_create_dummy2_instance()
+    check_ros2_environment()
+    
+    print("\n" + "=" * 60)
+    print("调试测试完成")
+    print("=" * 60)
+
+if __name__ == "__main__":
+    main()