mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2026-02-05 05:45:10 +00:00
226 lines
7.4 KiB
Python
226 lines
7.4 KiB
Python
#!/usr/bin/env python3
|
||
"""
|
||
Dummy2 MoveIt2控制测试（修复版本）
|
||
解决设备名称映射和action问题
|
||
"""
|
||
|
||
import json
|
||
import time
|
||
import sys
|
||
import os
|
||
import threading
|
||
import signal
|
||
|
||
# 添加Unilab路径
|
||
sys.path.insert(0, '/home/hh/Uni-Lab-OS')
|
||
|
||
def test_direct_moveit_action():
|
||
"""直接测试MoveIt action服务"""
|
||
print("🔧 直接测试MoveIt action服务...")
|
||
|
||
try:
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
from rclpy.action import ActionClient
|
||
from moveit_msgs.action import MoveGroup
|
||
from moveit_msgs.msg import (
|
||
MotionPlanRequest,
|
||
PlanningOptions,
|
||
Constraints,
|
||
JointConstraint
|
||
)
|
||
from geometry_msgs.msg import PoseStamped
|
||
|
||
# 初始化ROS2
|
||
rclpy.init()
|
||
|
||
# 创建节点
|
||
node = Node('moveit_test_client')
|
||
|
||
# 创建action客户端
|
||
action_client = ActionClient(node, MoveGroup, '/move_action')
|
||
|
||
# 启动executor
|
||
executor = rclpy.executors.MultiThreadedExecutor()
|
||
executor.add_node(node)
|
||
executor_thread = threading.Thread(target=executor.spin, daemon=True)
|
||
executor_thread.start()
|
||
|
||
print("✓ 节点和action客户端创建成功")
|
||
|
||
# 等待action服务
|
||
if not action_client.wait_for_server(timeout_sec=10.0):
|
||
print("❌ MoveIt action服务连接超时")
|
||
return False
|
||
|
||
print("✅ MoveIt action服务连接成功")
|
||
|
||
# 创建运动规划请求
|
||
goal_msg = MoveGroup.Goal()
|
||
|
||
# 设置请求参数
|
||
goal_msg.request.group_name = "dummy2_arm" # 注意这里的组名
|
||
goal_msg.request.num_planning_attempts = 3
|
||
goal_msg.request.allowed_planning_time = 5.0
|
||
goal_msg.request.max_velocity_scaling_factor = 0.2
|
||
goal_msg.request.max_acceleration_scaling_factor = 0.2
|
||
|
||
# 设置关节约束（简单的home位置）
|
||
joint_constraint = JointConstraint()
|
||
joint_constraint.joint_name = "Joint1"
|
||
joint_constraint.position = 0.0
|
||
joint_constraint.tolerance_above = 0.01
|
||
joint_constraint.tolerance_below = 0.01
|
||
joint_constraint.weight = 1.0
|
||
|
||
constraints = Constraints()
|
||
constraints.joint_constraints = [joint_constraint]
|
||
goal_msg.request.goal_constraints = [constraints]
|
||
|
||
# 设置规划选项
|
||
goal_msg.planning_options.planning_scene_diff.is_diff = True
|
||
goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = True
|
||
goal_msg.planning_options.plan_only = False # 执行规划结果
|
||
|
||
print("📤 发送MoveIt规划请求...")
|
||
print(f" 目标组: {goal_msg.request.group_name}")
|
||
print(f" 关节约束: {joint_constraint.joint_name} = {joint_constraint.position}")
|
||
|
||
# 发送目标
|
||
future = action_client.send_goal_async(goal_msg)
|
||
|
||
# 等待结果
|
||
rclpy.spin_until_future_complete(node, future, timeout_sec=3.0)
|
||
|
||
if future.result() is not None:
|
||
goal_handle = future.result()
|
||
if goal_handle.accepted:
|
||
print("✅ 规划请求被接受")
|
||
|
||
# 等待执行结果
|
||
result_future = goal_handle.get_result_async()
|
||
rclpy.spin_until_future_complete(node, result_future, timeout_sec=10.0)
|
||
|
||
if result_future.result() is not None:
|
||
result = result_future.result().result
|
||
print(f"📊 规划结果: {result.error_code.val}")
|
||
|
||
if result.error_code.val == 1: # SUCCESS
|
||
print("🎉 MoveIt规划和执行成功！")
|
||
return True
|
||
else:
|
||
print(f"❌ MoveIt执行失败，错误代码: {result.error_code.val}")
|
||
return False
|
||
else:
|
||
print("❌ 等待执行结果超时")
|
||
return False
|
||
else:
|
||
print("❌ 规划请求被拒绝")
|
||
return False
|
||
else:
|
||
print("❌ 发送规划请求超时")
|
||
return False
|
||
|
||
except Exception as e:
|
||
print(f"❌ MoveIt测试失败: {e}")
|
||
import traceback
|
||
traceback.print_exc()
|
||
return False
|
||
finally:
|
||
try:
|
||
executor.shutdown()
|
||
rclpy.shutdown()
|
||
except:
|
||
pass
|
||
|
||
def check_moveit_groups():
|
||
"""检查MoveIt规划组"""
|
||
print("\n🔍 检查MoveIt规划组...")
|
||
|
||
try:
|
||
import subprocess
|
||
|
||
# 获取规划组信息
|
||
result = subprocess.run([
|
||
'ros2', 'service', 'call', '/query_planner_params',
|
||
'moveit_msgs/srv/QueryPlannerParams', '{}'
|
||
], capture_output=True, text=True, timeout=10)
|
||
|
||
if result.returncode == 0:
|
||
print("✅ 成功查询规划器参数")
|
||
print("响应:")
|
||
print(result.stdout)
|
||
else:
|
||
print("❌ 查询规划器参数失败")
|
||
print(result.stderr)
|
||
|
||
except Exception as e:
|
||
print(f"❌ 检查规划组失败: {e}")
|
||
|
||
def check_robot_description():
|
||
"""检查机器人描述"""
|
||
print("\n🔍 检查机器人描述...")
|
||
|
||
try:
|
||
import subprocess
|
||
|
||
# 获取机器人描述参数
|
||
result = subprocess.run([
|
||
'ros2', 'param', 'get', '/move_group', 'robot_description'
|
||
], capture_output=True, text=True, timeout=10)
|
||
|
||
if result.returncode == 0:
|
||
urdf_content = result.stdout
|
||
# 检查关节名称
|
||
joint_names = []
|
||
for line in urdf_content.split('\n'):
|
||
if 'joint name=' in line and 'type=' in line:
|
||
# 简单解析关节名称
|
||
start = line.find('name="') + 6
|
||
end = line.find('"', start)
|
||
if start > 5 and end > start:
|
||
joint_name = line[start:end]
|
||
if 'Joint' in joint_name:
|
||
joint_names.append(joint_name)
|
||
|
||
print(f"✅ 找到关节: {joint_names}")
|
||
return joint_names
|
||
else:
|
||
print("❌ 获取机器人描述失败")
|
||
return []
|
||
|
||
except Exception as e:
|
||
print(f"❌ 检查机器人描述失败: {e}")
|
||
return []
|
||
|
||
def main():
|
||
"""主函数"""
|
||
print("🔧 MoveIt2控制测试（修复版本）")
|
||
print("=" * 50)
|
||
|
||
# 1. 检查机器人描述和关节
|
||
joint_names = check_robot_description()
|
||
|
||
# 2. 检查规划组
|
||
check_moveit_groups()
|
||
|
||
# 3. 直接测试MoveIt action
|
||
print("\n" + "="*30)
|
||
print("开始MoveIt Action测试")
|
||
print("="*30)
|
||
|
||
if test_direct_moveit_action():
|
||
print("\n🎉 MoveIt2控制测试成功！")
|
||
print("Dummy2可以通过MoveIt2进行规划和控制")
|
||
else:
|
||
print("\n❌ MoveIt2控制测试失败")
|
||
print("需要进一步调试配置问题")
|
||
|
||
print("\n📋 下一步建议:")
|
||
print("1. 检查SRDF文件中的规划组配置")
|
||
print("2. 验证关节名称映射")
|
||
print("3. 调试运动学求解器配置")
|
||
|
||
if __name__ == "__main__":
|
||
main()
|