fix:debug dummy2

dummy2_debug/fix_moveit_config.py

@@ -0,0 +1,245 @@
+#!/usr/bin/env python3
+"""
+MoveIt2配置问题诊断和修复脚本
+"""
+
+import subprocess
+import time
+import sys
+import os
+
+def check_current_services():
+    """检查当前ROS2服务状态"""
+    print("🔍 检查当前ROS2服务状态...")
+    
+    try:
+        # 检查节点
+        result = subprocess.run(['ros2', 'node', 'list'], 
+                               capture_output=True, text=True, timeout=5)
+        if result.returncode == 0:
+            nodes = result.stdout.strip().split('\n')
+            print(f"当前运行的节点 ({len(nodes)}):")
+            for node in nodes:
+                print(f"  - {node}")
+                
+            # 检查是否有move_group
+            if '/move_group' in nodes:
+                print("✅ move_group节点正在运行")
+                return True
+            else:
+                print("❌ move_group节点未运行")
+                return False
+        else:
+            print("❌ 无法获取节点列表")
+            return False
+            
+    except Exception as e:
+        print(f"❌ 检查服务状态失败: {e}")
+        return False
+
+def check_moveit_launch_files():
+    """检查MoveIt启动文件"""
+    print("\n🔍 检查MoveIt启动文件...")
+    
+    dummy2_ws = "/home/hh/dummy2/ros2/dummy2_ws"
+    
+    # 检查demo.launch.py
+    demo_files = [
+        f"{dummy2_ws}/install/dummy2_moveit_config/share/dummy2_moveit_config/launch/demo.launch.py",
+        f"{dummy2_ws}/src/dummy2_moveit_config/launch/demo.launch.py"
+    ]
+    
+    for demo_file in demo_files:
+        if os.path.exists(demo_file):
+            print(f"✅ 找到demo.launch.py: {demo_file}")
+            return demo_file
+    
+    print("❌ 未找到demo.launch.py")
+    return None
+
+def start_moveit_service():
+    """启动MoveIt服务"""
+    print("\n🚀 启动MoveIt2服务...")
+    
+    dummy2_ws = "/home/hh/dummy2/ros2/dummy2_ws"
+    
+    try:
+        # 设置环境
+        env = os.environ.copy()
+        env['ROS_DISTRO'] = 'humble'
+        
+        # 切换到工作空间
+        os.chdir(dummy2_ws)
+        
+        # 构建启动命令
+        cmd = [
+            'bash', '-c',
+            'source /opt/ros/humble/setup.bash && '
+            'source install/setup.bash && '
+            'ros2 launch dummy2_moveit_config demo.launch.py'
+        ]
+        
+        print("执行命令:", ' '.join(cmd))
+        print("⚠️  这将启动MoveIt2服务，按Ctrl+C停止")
+        
+        # 启动服务
+        process = subprocess.Popen(cmd, env=env)
+        process.wait()
+        
+    except KeyboardInterrupt:
+        print("\n⚠️  用户中断服务")
+    except Exception as e:
+        print(f"❌ 启动MoveIt服务失败: {e}")
+
+def test_moveit_actions():
+    """测试MoveIt action服务"""
+    print("\n🧪 测试MoveIt action服务...")
+    
+    try:
+        # 等待服务启动
+        time.sleep(3)
+        
+        # 检查action列表
+        result = subprocess.run(['ros2', 'action', 'list'], 
+                               capture_output=True, text=True, timeout=10)
+        if result.returncode == 0:
+            actions = result.stdout.strip().split('\n')
+            print(f"可用的action服务 ({len(actions)}):")
+            for action in actions:
+                print(f"  - {action}")
+                
+            # 查找MoveIt相关actions
+            moveit_actions = [a for a in actions if 'move' in a.lower()]
+            if moveit_actions:
+                print(f"\nMoveIt相关actions:")
+                for action in moveit_actions:
+                    print(f"  ✅ {action}")
+                return True
+            else:
+                print("❌ 未找到MoveIt相关actions")
+                return False
+        else:
+            print("❌ 无法获取action列表")
+            return False
+            
+    except Exception as e:
+        print(f"❌ 测试action服务失败: {e}")
+        return False
+
+def create_moveit_fix_script():
+    """创建MoveIt修复脚本"""
+    print("\n📝 创建MoveIt修复脚本...")
+    
+    script_content = """#!/bin/bash
+# MoveIt2服务启动脚本
+
+DUMMY2_WS="/home/hh/dummy2/ros2/dummy2_ws"
+
+echo "🚀 启动MoveIt2服务..."
+echo "工作空间: $DUMMY2_WS"
+
+cd "$DUMMY2_WS"
+
+# 设置环境
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+
+echo "📋 可用的启动文件:"
+find install/ -name "*.launch.py" | grep moveit | head -5
+
+echo ""
+echo "🎯 启动move_group服务..."
+echo "命令: ros2 launch dummy2_moveit_config move_group.launch.py"
+
+# 启动move_group
+ros2 launch dummy2_moveit_config move_group.launch.py
+"""
+    
+    script_path = "/home/hh/Uni-Lab-OS/dummy2_debug/start_moveit.sh"
+    with open(script_path, 'w') as f:
+        f.write(script_content)
+    
+    # 设置可执行权限
+    os.chmod(script_path, 0o755)
+    print(f"✅ 创建脚本: {script_path}")
+    
+    return script_path
+
+def diagnose_moveit_config():
+    """诊断MoveIt配置"""
+    print("\n🔧 诊断MoveIt配置问题...")
+    
+    # 检查配置文件
+    dummy2_ws = "/home/hh/dummy2/ros2/dummy2_ws"
+    config_dirs = [
+        f"{dummy2_ws}/install/dummy2_moveit_config/share/dummy2_moveit_config/config",
+        f"{dummy2_ws}/src/dummy2_moveit_config/config"
+    ]
+    
+    for config_dir in config_dirs:
+        if os.path.exists(config_dir):
+            print(f"✅ 找到配置目录: {config_dir}")
+            
+            # 列出配置文件
+            config_files = os.listdir(config_dir)
+            print("配置文件:")
+            for file in config_files[:10]:  # 只显示前10个
+                print(f"  - {file}")
+            break
+    else:
+        print("❌ 未找到MoveIt配置目录")
+    
+    # 检查URDF文件
+    urdf_dirs = [
+        f"{dummy2_ws}/install/dummy2_description/share/dummy2_description",
+        f"{dummy2_ws}/src/dummy2_description"
+    ]
+    
+    for urdf_dir in urdf_dirs:
+        if os.path.exists(urdf_dir):
+            print(f"✅ 找到URDF目录: {urdf_dir}")
+            break
+    else:
+        print("❌ 未找到URDF目录")
+
+def main():
+    """主函数"""
+    print("🔧 MoveIt2配置诊断工具")
+    print("=" * 50)
+    
+    # 1. 检查当前状态
+    move_group_running = check_current_services()
+    
+    # 2. 诊断配置
+    diagnose_moveit_config()
+    
+    # 3. 检查启动文件
+    demo_file = check_moveit_launch_files()
+    
+    # 4. 创建修复脚本
+    fix_script = create_moveit_fix_script()
+    
+    print("\n" + "=" * 50)
+    print("📋 诊断结果总结")
+    print("=" * 50)
+    
+    if move_group_running:
+        print("✅ MoveIt2服务正在运行")
+        test_moveit_actions()
+    else:
+        print("❌ MoveIt2服务未运行")
+        print("\n🔧 解决方案:")
+        print("1. 使用修复脚本启动MoveIt:")
+        print(f"   {fix_script}")
+        print("\n2. 或手动启动:")
+        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 move_group.launch.py")
+        
+        print("\n3. 在新终端测试Unilab控制:")
+        print("   cd /home/hh/Uni-Lab-OS/dummy2_debug")
+        print("   python dummy2_move_demo.py")
+
+if __name__ == "__main__":
+    main()