feat: Remove ROS2 dependency, enhance arming with retries, and improve takeoff sequence with better altitude monitoring and arming failure handling.

scripts/run_ardupilot.py

@@ -31,14 +31,6 @@ except ImportError:
     print("Run: pip install pymavlink")
     sys.exit(1)
 
-try:
-    import rclpy
-    from rclpy.node import Node
-    ROS2_AVAILABLE = True
-except ImportError:
-    ROS2_AVAILABLE = False
-    print("[WARN] ROS 2 (rclpy) not available - using standalone controller")
-
 from config import ARDUPILOT, CONTROLLER, MAVLINK
 
 
@@ -83,10 +75,11 @@ class ArduPilotInterface:
             
             if msg_type == "HEARTBEAT":
                 self.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
-                # Get mode name
-                if hasattr(mavutil.mavlink, 'enums'):
-                    mode_map = mavutil.mavlink.enums.get('COPTER_MODE', {})
-                    self.mode = mode_map.get(msg.custom_mode, {}).get('name', str(msg.custom_mode))
+                # Get mode name using mavutil helper
+                try:
+                    self.mode = mavutil.mode_string_v10(msg)
+                except Exception:
+                    self.mode = str(msg.custom_mode)
                 
             elif msg_type == "LOCAL_POSITION_NED":
                 self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
@@ -135,22 +128,32 @@ class ArduPilotInterface:
     
     def arm(self, force=True):
         """Arm motors."""
-        if force:
-            # Force arm (bypass checks)
-            self.mav.mav.command_long_send(
-                self.mav.target_system,
-                self.mav.target_component,
-                mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-                0, 1, 21196, 0, 0, 0, 0, 0  # 21196 = force arm
-            )
-        else:
-            self.mav.arducopter_arm()
+        print("[INFO] Arming...")
         
-        time.sleep(1)
-        self.update_telemetry()
-        if self.armed:
-            print("[OK] Armed")
-        return self.armed
+        for attempt in range(3):
+            if force:
+                # Force arm (bypass checks)
+                self.mav.mav.command_long_send(
+                    self.mav.target_system,
+                    self.mav.target_component,
+                    mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+                    0, 1, 21196, 0, 0, 0, 0, 0  # 21196 = force arm
+                )
+            else:
+                self.mav.arducopter_arm()
+            
+            # Wait and check if armed
+            time.sleep(1)
+            self.update_telemetry()
+            
+            if self.armed:
+                print("[OK] Armed")
+                return True
+            else:
+                print(f"[WARN] Arm attempt {attempt + 1}/3 failed, retrying...")
+        
+        print("[ERROR] Failed to arm after 3 attempts")
+        return False
     
     def disarm(self):
         """Disarm motors."""
@@ -279,22 +282,30 @@ def main():
         # Set GUIDED mode and arm
         print("[INFO] Setting up for takeoff...")
         ardupilot.set_mode("GUIDED")
-        time.sleep(1)
+        time.sleep(2)  # Give mode time to change
         
-        ardupilot.arm(force=True)
-        time.sleep(1)
+        # Update telemetry to get current mode
+        ardupilot.update_telemetry()
+        print(f"[INFO] Current mode: {ardupilot.mode}")
         
-        if ardupilot.armed:
+        if not ardupilot.arm(force=True):
+            print("[ERROR] Could not arm - continuing in manual mode")
+            print("[INFO] Use MAVProxy to arm: arm throttle force")
+        else:
             ardupilot.takeoff(args.takeoff_alt)
             print(f"[INFO] Taking off to {args.takeoff_alt}m...")
             
-            # Wait for takeoff
-            for _ in range(50):  # 5 seconds
+            # Wait for takeoff (up to 15 seconds)
+            for i in range(150):
                 telemetry = ardupilot.get_telemetry()
-                if telemetry["altimeter"]["altitude"] > args.takeoff_alt * 0.9:
-                    print("[OK] Reached target altitude")
+                alt = telemetry["altimeter"]["altitude"]
+                if alt > args.takeoff_alt * 0.9:
+                    print(f"\n[OK] Reached target altitude ({alt:.1f}m)")
                     break
+                if i % 10 == 0:
+                    print(f"\r[INFO] Climbing... {alt:.1f}m / {args.takeoff_alt}m", end="")
                 time.sleep(0.1)
+            print("")
     
     print("\n[INFO] Starting controller loop...")
     print("[INFO] Press Ctrl+C to stop\n")