mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2026-02-05 05:45:10 +00:00
246 lines
7.1 KiB
Python
246 lines
7.1 KiB
Python
#!/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()
|