sync recent changes

2026-05-18 18:20:08 +00:00 · 2025-11-21 14:13:05 +01:00
parent e5cae6be64
commit 9a052566a3
326 changed files with 20122 additions and 15 deletions
@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+"""
+Test script to check G1 robot's current mode and print it to screen.
+This script uses MotionSwitcherClient to check the robot's control mode.
+"""
+
+import time
+import sys
+import json
+
+# Add the unitree_sdk2_python path to sys.path
+sys.path.append('/Users/nepyope/Documents/unitree/unitree_IL_lerobot/unitree_sdk2_python')
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+
+
+class RobotModeChecker:
+    def __init__(self):
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+    
+    def check_mode(self):
+        """Check the robot's current mode and return the result."""
+        try:
+            status, result = self.msc.CheckMode()
+            return status, result
+        except Exception as e:
+            print(f"Error checking mode: {e}")
+            return -1, None
+    
+    def print_mode_info(self):
+        """Print detailed mode information to screen."""
+        print("=" * 50)
+        print("G1 Robot Mode Checker")
+        print("=" * 50)
+        
+        print("\nChecking robot mode...")
+        status, result = self.check_mode()
+        
+        if status == 0 and result is not None:
+            print("✅ Successfully connected to robot!")
+            print(f"📊 Status Code: {status}")
+            print(f"📋 Mode Result: {json.dumps(result, indent=2)}")
+            
+            # Extract mode name if available
+            if isinstance(result, dict) and 'name' in result:
+                mode_name = result['name']
+                if mode_name:
+                    print(f"🤖 Current Mode: {mode_name}")
+                    
+                    # Provide mode interpretation
+                    mode_interpretations = {
+                        'ai': 'AI/Autonomous Mode - Ready for external commands',
+                        'normal': 'Normal Mode - Basic operation mode',
+                        'advanced': 'Advanced Mode - Advanced control mode',
+                        'ai-w': 'AI-Wheeled Mode - For wheeled robots',
+                        '': 'Safe Mode - No active control mode'
+                    }
+                    
+                    interpretation = mode_interpretations.get(mode_name, 'Unknown Mode')
+                    print(f"💡 Interpretation: {interpretation}")
+                else:
+                    print("🤖 Current Mode: Safe Mode (No active mode)")
+                    print("💡 Interpretation: Robot is in safe state, ready to accept commands")
+            else:
+                print("🤖 Current Mode: Unknown (check result structure)")
+                
+        elif status == -1:
+            print("❌ Failed to connect to robot")
+            print("🔧 Troubleshooting:")
+            print("   - Check network connection")
+            print("   - Verify robot is powered on")
+            print("   - Ensure correct network interface (try en7)")
+            print("   - Check if robot IP is 192.168.123.164")
+        else:
+            print(f"❌ Error checking mode. Status: {status}")
+            if result:
+                print(f"📋 Result: {result}")
+        
+        print("\n" + "=" * 50)
+
+
+def main():
+    print("G1 Robot Mode Checker")
+    print("This script will check the robot's current control mode.")
+    
+    # Get network interface from command line or use default
+    if len(sys.argv) > 1:
+        network_interface = sys.argv[1]
+        print(f"Using network interface: {network_interface}")
+    else:
+        network_interface = "en7"  # Default based on your setup
+        print(f"Using default network interface: {network_interface}")
+    
+    print("\nInitializing connection...")
+    
+    try:
+        # Initialize the channel factory
+        ChannelFactoryInitialize(0, network_interface)
+        print("✅ Channel factory initialized")
+        
+        # Create mode checker
+        checker = RobotModeChecker()
+        print("✅ MotionSwitcherClient created")
+        
+        # Check and print mode
+        checker.print_mode_info()
+        
+        # Optionally check mode multiple times
+        print("\n🔄 Checking mode again in 2 seconds...")
+        time.sleep(2)
+        checker.print_mode_info()
+        
+    except Exception as e:
+        print(f"❌ Error during initialization: {e}")
+        print("\n🔧 Troubleshooting:")
+        print("   - Make sure you're connected to the robot's network")
+        print("   - Try: sudo ifconfig en7 192.168.123.100 netmask 255.255.255.0")
+        print("   - Verify robot is powered on and accessible")
+
+
+if __name__ == "__main__":
+    main()