fix:debug dummy2

dummy2_debug/test_dummy2_real_control.py

@@ -0,0 +1,207 @@
+#!/usr/bin/env python3
+"""
+Dummy2 Unilab实际控制测试
+需要先启动ROS2服务，然后测试通过Unilab控制Dummy2
+"""
+
+import json
+import time
+import sys
+import os
+import threading
+
+# 添加Unilab路径
+sys.path.insert(0, '/home/hh/Uni-Lab-OS')
+
+def check_ros2_services():
+    """检查ROS2服务状态"""
+    print("=" * 50)
+    print("检查ROS2服务状态")
+    print("=" * 50)
+    
+    try:
+        import subprocess
+        import rclpy
+        
+        # 初始化ROS2
+        rclpy.init()
+        
+        # 检查话题
+        result = subprocess.run(['ros2', 'topic', 'list'], 
+                               capture_output=True, text=True, timeout=5)
+        if result.returncode == 0:
+            topics = result.stdout.strip().split('\n')
+            print(f"✓ 发现 {len(topics)} 个ROS2话题")
+            
+            # 查找dummy2相关话题
+            dummy2_topics = [t for t in topics if 'dummy2' in t.lower()]
+            if dummy2_topics:
+                print("Dummy2相关话题:")
+                for topic in dummy2_topics[:5]:  # 只显示前5个
+                    print(f"  {topic}")
+            else:
+                print("✗ 未发现Dummy2相关话题")
+        else:
+            print("✗ 无法获取ROS2话题列表")
+            
+        # 检查服务
+        result = subprocess.run(['ros2', 'service', 'list'], 
+                               capture_output=True, text=True, timeout=5)
+        if result.returncode == 0:
+            services = result.stdout.strip().split('\n')
+            print(f"✓ 发现 {len(services)} 个ROS2服务")
+            
+            # 查找MoveIt相关服务
+            moveit_services = [s for s in services if 'moveit' in s.lower()]
+            if moveit_services:
+                print("MoveIt相关服务:")
+                for service in moveit_services[:5]:  # 只显示前5个
+                    print(f"  {service}")
+            else:
+                print("✗ 未发现MoveIt相关服务")
+        else:
+            print("✗ 无法获取ROS2服务列表")
+            
+        rclpy.shutdown()
+        return True
+        
+    except subprocess.TimeoutExpired:
+        print("✗ ROS2命令超时")
+        return False
+    except Exception as e:
+        print(f"✗ 检查ROS2服务失败: {e}")
+        return False
+
+def test_actual_moveit_control():
+    """测试实际的MoveIt控制"""
+    print("\n" + "=" * 50)
+    print("测试实际MoveIt控制")
+    print("=" * 50)
+    
+    try:
+        import rclpy
+        from rclpy.node import Node
+        from unilabos.devices.ros_dev.moveit_interface import MoveitInterface
+        
+        # 初始化ROS2
+        rclpy.init()
+        
+        # 创建节点
+        test_node = Node("dummy2_test_node")
+        test_node.device_id = "dummy2_test"
+        test_node.callback_group = rclpy.callback_groups.ReentrantCallbackGroup()
+        
+        # 启动executor
+        executor = rclpy.executors.MultiThreadedExecutor()
+        executor.add_node(test_node)
+        executor_thread = threading.Thread(target=executor.spin, daemon=True)
+        executor_thread.start()
+        
+        print("✓ 测试节点创建成功")
+        
+        # 创建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
+        )
+        
+        # 执行post_init
+        moveit_interface.post_init(test_node)
+        print("✓ MoveitInterface初始化完成")
+        
+        # 等待服务可用
+        print("等待MoveIt服务...")
+        time.sleep(3)
+        
+        # 测试关节运动（安全位置）
+        print("测试关节运动到安全位置...")
+        safe_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        
+        try:
+            result = moveit_interface.moveit_joint_task(
+                move_group='arm',
+                joint_positions=safe_positions,
+                speed=0.1,  # 慢速运动
+                retry=3
+            )
+            
+            if result:
+                print("✓ 关节运动成功执行")
+            else:
+                print("✗ 关节运动执行失败")
+                
+        except Exception as e:
+            print(f"✗ 关节运动测试失败: {e}")
+        
+        # 清理
+        executor.shutdown()
+        executor_thread.join(timeout=2)
+        rclpy.shutdown()
+        
+        return True
+        
+    except Exception as e:
+        print(f"✗ 实际控制测试失败: {e}")
+        import traceback
+        traceback.print_exc()
+        return False
+
+def print_startup_instructions():
+    """打印启动说明"""
+    print("\n" + "=" * 60)
+    print("Dummy2 ROS2服务启动说明")
+    print("=" * 60)
+    
+    print("在测试Unilab控制之前，需要先启动ROS2服务：")
+    print("\n1. 打开新终端，导航到dummy2_ws:")
+    print("   cd /home/hh/dummy2/ros2/dummy2_ws")
+    
+    print("\n2. 设置ROS2环境:")
+    print("   source /opt/ros/humble/setup.bash")
+    print("   source install/setup.bash")
+    
+    print("\n3. 启动dummy2硬件接口:")
+    print("   ros2 launch dummy2_hw dummy2_hw.launch.py")
+    
+    print("\n4. 在另一个终端启动MoveIt2:")
+    print("   cd /home/hh/dummy2/ros2/dummy2_ws")
+    print("   source /opt/ros/humble/setup.bash")
+    print("   source install/setup.bash")
+    print("   ros2 launch dummy2_moveit_config demo.launch.py")
+    
+    print("\n5. 然后回到这里运行实际控制测试:")
+    print("   python test_dummy2_real_control.py --test-control")
+    
+    print("\n注意事项:")
+    print("- 确保dummy2硬件连接正常")
+    print("- 检查CAN2ETH网络连接")
+    print("- 确保机械臂处于安全位置")
+
+def main():
+    """主函数"""
+    if '--test-control' in sys.argv:
+        # 实际控制测试模式
+        print("Dummy2实际控制测试")
+        print("=" * 60)
+        
+        if check_ros2_services():
+            test_actual_moveit_control()
+        else:
+            print("\n请先启动ROS2服务后再测试")
+            print_startup_instructions()
+    else:
+        # 检查模式
+        print("Dummy2 ROS2服务检查")
+        print("=" * 60)
+        
+        if check_ros2_services():
+            print("\n✓ ROS2服务运行正常，可以进行实际控制测试")
+            print("运行以下命令进行实际控制测试:")
+            print("python test_dummy2_real_control.py --test-control")
+        else:
+            print("\n需要先启动ROS2服务")
+            print_startup_instructions()
+
+if __name__ == "__main__":
+    main()