add some debug about dummy2

dummy2_debug/test_unilab_moveit_final.py

@@ -0,0 +1,182 @@
+#!/usr/bin/env python3
+"""
+Final Unilab MoveIt2 Integration Test
+测试完整的 Unilab-MoveIt2 集成
+"""
+
+import sys
+import os
+import time
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from moveit_msgs.action import MoveGroup
+from moveit_msgs.msg import (
+    MotionPlanRequest,
+    Constraints,
+    JointConstraint,
+    PlanningOptions,
+    WorkspaceParameters
+)
+from geometry_msgs.msg import Vector3
+import threading
+
+# 添加 Unilab 路径
+sys.path.append('/home/hh/Uni-Lab-OS')
+from unilabos.devices.ros_dev.moveit_interface import MoveitInterface
+
+class FinalUnilabTest(Node):
+    def __init__(self):
+        super().__init__('final_unilab_test')
+        self.action_client = ActionClient(self, MoveGroup, '/move_action')
+        self.moveit_interface = MoveitInterface()
+        
+        # 初始化完成后再设置设备 ID
+        self.moveit_interface.device_id = "dummy2"
+        
+        print("🔧 等待 MoveIt2 动作服务...")
+        if not self.action_client.wait_for_server(timeout_sec=10.0):
+            print("❌ MoveIt2 动作服务不可用")
+            return
+        print("✅ MoveIt2 动作服务已连接")
+        
+    def test_joint_movement(self):
+        """测试关节空间运动"""
+        print("\n🎯 测试关节空间运动...")
+        
+        # 使用 Unilab MoveitInterface 的方法
+        try:
+            target_joints = {
+                'joint_1': 0.1,
+                'joint_2': 0.0,
+                'joint_3': 0.0,
+                'joint_4': 0.0,
+                'joint_5': 0.0,
+                'joint_6': 0.0
+            }
+            
+            print(f"📤 发送关节目标: {target_joints}")
+            result = self.moveit_interface.moveit_joint_task(target_joints)
+            print(f"✅ 运动结果: {result}")
+            return True
+            
+        except Exception as e:
+            print(f"❌ 运动失败: {e}")
+            return False
+    
+    def test_direct_action(self):
+        """直接测试 MoveIt 动作"""
+        print("\n🎯 直接测试 MoveIt 动作...")
+        
+        # 创建运动规划请求
+        goal_msg = MoveGroup.Goal()
+        goal_msg.request = MotionPlanRequest()
+        
+        # 设置规划组
+        goal_msg.request.group_name = "dummy2_arm"
+        
+        # 设置关节约束
+        joint_constraint = JointConstraint()
+        joint_constraint.joint_name = "Joint1"  # 使用实际的关节名称
+        joint_constraint.position = 0.1
+        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 = PlanningOptions()
+        goal_msg.planning_options.plan_only = False  # 规划并执行
+        goal_msg.planning_options.look_around = False
+        goal_msg.planning_options.look_around_attempts = 0
+        goal_msg.planning_options.max_safe_execution_cost = 1.0
+        goal_msg.planning_options.replan = False
+        goal_msg.planning_options.replan_attempts = 0
+        goal_msg.planning_options.replan_delay = 0.0
+        
+        # 设置工作空间
+        goal_msg.request.workspace_parameters = WorkspaceParameters()
+        goal_msg.request.workspace_parameters.header.frame_id = "base_link"
+        goal_msg.request.workspace_parameters.min_corner = Vector3(x=-1.0, y=-1.0, z=-1.0)
+        goal_msg.request.workspace_parameters.max_corner = Vector3(x=1.0, y=1.0, z=1.0)
+        
+        # 设置允许的规划时间
+        goal_msg.request.allowed_planning_time = 5.0
+        goal_msg.request.num_planning_attempts = 1
+        
+        print("📤 发送规划和执行请求...")
+        future = self.action_client.send_goal_async(goal_msg)
+        
+        try:
+            rclpy.spin_until_future_complete(self, future, timeout_sec=10.0)
+            goal_handle = future.result()
+            
+            if not goal_handle.accepted:
+                print("❌ 目标被拒绝")
+                return False
+                
+            print("✅ 目标被接受，等待执行结果...")
+            result_future = goal_handle.get_result_async()
+            rclpy.spin_until_future_complete(self, result_future, timeout_sec=30.0)
+            
+            result = result_future.result().result
+            print(f"📊 执行结果错误码: {result.error_code.val}")
+            
+            if result.error_code.val == 1:  # SUCCESS
+                print("🎉 运动成功!")
+                return True
+            else:
+                print(f"❌ 运动失败，错误码: {result.error_code.val}")
+                return False
+                
+        except Exception as e:
+            print(f"❌ 执行异常: {e}")
+            return False
+
+def main():
+    print("🤖 Unilab MoveIt2 最终集成测试")
+    print("=" * 50)
+    
+    # 初始化 ROS2
+    rclpy.init()
+    
+    try:
+        # 创建测试节点
+        test_node = FinalUnilabTest()
+        
+        # 运行测试
+        print("\n🚀 开始测试序列...")
+        
+        # 测试1: Unilab MoveitInterface
+        success1 = test_node.test_joint_movement()
+        time.sleep(2)
+        
+        # 测试2: 直接 MoveIt 动作
+        success2 = test_node.test_direct_action()
+        
+        # 结果总结
+        print("\n" + "=" * 50)
+        print("📋 测试结果总结:")
+        print(f"  Unilab 接口测试: {'✅ 成功' if success1 else '❌ 失败'}")
+        print(f"  直接动作测试: {'✅ 成功' if success2 else '❌ 失败'}")
+        
+        if success1 or success2:
+            print("\n🎉 集成测试部分成功! Dummy2 可以通过 Unilab 控制")
+        else:
+            print("\n⚠️  需要进一步调试配置")
+            
+    except KeyboardInterrupt:
+        print("\n⚠️  用户中断测试")
+    except Exception as e:
+        print(f"\n❌ 测试异常: {e}")
+    finally:
+        try:
+            rclpy.shutdown()
+        except:
+            pass
+
+if __name__ == '__main__':
+    main()