fix dummy2

dummy2_debug/force_home.py

@@ -0,0 +1,106 @@
+#!/usr/bin/env python3
+"""
+强制回到Home位置
+确保机械臂真正回到零位
+"""
+
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from control_msgs.action import FollowJointTrajectory
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+import time
+
+class ForceHome(Node):
+    def __init__(self):
+        super().__init__('force_home')
+        self.client = ActionClient(self, FollowJointTrajectory, '/dummy2_arm_controller/follow_joint_trajectory')
+        
+        print("🏠 强制回到Home位置...")
+        if not self.client.wait_for_server(timeout_sec=5.0):
+            print("❌ 控制器服务不可用")
+            return
+        print("✅ 控制器已连接")
+        
+    def go_home(self, duration=8):
+        """强制回到home位置"""
+        print(f"🎯 执行回home运动 (时长: {duration}秒)...")
+        
+        goal_msg = FollowJointTrajectory.Goal()
+        goal_msg.trajectory = JointTrajectory()
+        goal_msg.trajectory.header.frame_id = ""
+        goal_msg.trajectory.joint_names = ["Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"]
+        
+        # 明确的零位点
+        point = JointTrajectoryPoint()
+        point.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  # 明确的home位置
+        point.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  # 停止速度
+        point.time_from_start.sec = duration
+        point.time_from_start.nanosec = 0
+        
+        goal_msg.trajectory.points = [point]
+        
+        print("📤 发送回home指令...")
+        future = self.client.send_goal_async(goal_msg)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
+        
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            print("❌ 回home指令被拒绝")
+            return False
+            
+        print("✅ 回home指令已接受，机械臂运动中...")
+        print(f"⏱️  等待 {duration} 秒执行完成...")
+        
+        result_future = goal_handle.get_result_async()
+        rclpy.spin_until_future_complete(self, result_future, timeout_sec=duration+2)
+        
+        result = result_future.result().result
+        print(f"📊 执行结果错误码: {result.error_code}")
+        
+        if result.error_code == 0:
+            print("🎉 成功回到Home位置!")
+            return True
+        else:
+            print(f"⚠️  执行警告，错误码: {result.error_code}")
+            return False
+
+def main():
+    print("🏠 Dummy2强制回Home程序")
+    print("=" * 40)
+    
+    rclpy.init()
+    
+    try:
+        home_node = ForceHome()
+        
+        print("⚠️  即将执行回home运动，请确保机械臂周围安全")
+        print("⚠️  可随时按 Ctrl+C 紧急停止")
+        print()
+        
+        input("按Enter键开始回home...")
+        
+        success = home_node.go_home(duration=10)  # 给更长时间确保到位
+        
+        if success:
+            print("\n🎉 机械臂应该已回到Home位置")
+            print("💡 请检查机械臂是否在零位")
+        else:
+            print("\n⚠️  回home过程有警告，请检查机械臂状态")
+            
+        # 等待一下再检查状态
+        print("\n等待2秒后检查关节状态...")
+        time.sleep(2)
+        
+    except KeyboardInterrupt:
+        print("\n⚠️  用户中断操作")
+    except Exception as e:
+        print(f"\n❌ 异常: {e}")
+    finally:
+        try:
+            rclpy.shutdown()
+        except:
+            pass
+
+if __name__ == '__main__':
+    main()