fix:debug dummy2

dummy2_debug/test_moveit_action.py

@@ -0,0 +1,225 @@
+#!/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()