ArduPilot SITL Update

README.md

@@ -1,154 +1,107 @@
 # Drone Landing Simulation (GPS-Denied)
 
-A GPS-denied drone landing simulation using relative sensors (IMU, altimeter, camera, landing pad detection) with multiple backends:
+A GPS-denied drone landing simulation with multiple backends. Land a drone on a moving platform using only relative sensors (IMU, altimeter, camera).
-- **PyBullet** - Lightweight physics simulation
-- **Gazebo** - Full robotics simulator
-- **ArduPilot SITL** - Realistic flight controller with MAVProxy
 
 ## Quick Start
 
-### Standalone Mode (Any Platform - No ROS 2 Required)
+### Standalone Mode (No ROS 2 - Any Platform)
 
 ```bash
-source activate.sh  # Linux/macOS
+source activate.sh
-. .\activate.ps1    # Windows
-
 python standalone_simulation.py --pattern circular --speed 0.3
 ```
 
-### PyBullet + ROS 2 (Two Terminals)
+### Gazebo + ROS 2 (Linux/WSL2)
 
-**Terminal 1 - Simulator:**
+**Terminal 1:**
-```bash
-python simulation_host.py
-```
-
-**Terminal 2 - Bridge + Controllers:**
-```bash
-python run_bridge.py --pattern circular --speed 0.3
-```
-
-### Gazebo + ROS 2 (Two Terminals - Linux/WSL2)
-
-**Terminal 1 - Launch Gazebo + Bridge:**
 ```bash
 ros2 launch gazebo/launch/drone_landing.launch.py
 ```
 
-**Terminal 2 - Run Controllers:**
+**Terminal 2:**
 ```bash
-python run_gazebo.py --pattern circular --speed 0.3
+source activate.sh
+python run_gazebo.py --pattern circular
+python camera_viewer.py  # Optional: view camera feed
 ```
 
-### ArduPilot SITL + Gazebo (Three Terminals - Realistic Flight Controller)
+### ArduPilot SITL (Realistic Flight Controller)
 
-**Terminal 1 - Launch Gazebo with ArduPilot world:**
+**Terminal 1:**
 ```bash
-ros2 launch gazebo/launch/ardupilot_drone.launch.py
+source ~/ardu_ws/install/setup.bash
+ros2 launch ardupilot_gz_bringup iris_runway.launch.py
 ```
 
-**Terminal 2 - Start ArduPilot SITL:**
+**Terminal 2:**
 ```bash
-cd ~/ardupilot
+mavproxy.py --console --map --master=:14550
-sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
-```
-
-**Terminal 3 - Run MAVLink Bridge + Controllers:**
-```bash
-python run_ardupilot.py --no-sitl --pattern circular
 ```
 
 ## Installation
 
-| Platform | Command |
+```bash
-|----------|---------|
+# Ubuntu/Debian (includes ROS 2 + Gazebo)
+./setup/install_ubuntu.sh
+
+# With ArduPilot SITL
+./setup/install_ardupilot.sh
+
+# Activate environment
+source activate.sh
+```
+
+| Platform | Install Script |
+|----------|---------------|
 | Ubuntu/Debian | `./setup/install_ubuntu.sh` |
+| ArduPilot SITL | `./setup/install_ardupilot.sh` |
 | Arch Linux | `./setup/install_arch.sh` |
 | macOS | `./setup/install_macos.sh` |
 | Windows | `.\setup\install_windows.ps1` |
 
-## Platform Compatibility
-
-| Feature | Ubuntu | Arch | macOS | Windows | WSL2 |
-|---------|--------|------|-------|---------|------|
-| Standalone | ✅ | ✅ | ✅ | ✅ | ✅ |
-| PyBullet + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ |
-| Gazebo + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ |
-| ArduPilot + Gazebo | ✅ | ⚠️ | ❌ | ❌ | ✅ |
-
 ## Files
 
 | File | Description |
 |------|-------------|
-| `standalone_simulation.py` | All-in-one (no ROS 2 required) |
+| `standalone_simulation.py` | **All-in-one** (no ROS 2) |
-| `simulation_host.py` | PyBullet simulator server |
+| `run_gazebo.py` | Gazebo controllers |
-| `run_bridge.py` | PyBullet bridge + Controllers |
+| `run_ardupilot.py` | ArduPilot launcher |
-| `run_gazebo.py` | Gazebo bridge + Controllers |
+| `camera_viewer.py` | Drone camera window |
-| `run_ardupilot.py` | **ArduPilot SITL** + MAVLink bridge |
+| `drone_controller.py` | **Your landing algorithm** |
-| `mavlink_bridge.py` | MAVLink ↔ ROS 2 bridge |
+| `config.py` | Configuration |
-| `config.py` | **Configuration file** (edit to customize) |
+
-| `drone_controller.py` | **Your landing algorithm** (edit this!) |
+## Sensors Available
-| `rover_controller.py` | Moving landing pad controller |
+
+| Sensor | Data |
+|--------|------|
+| IMU | Orientation (roll, pitch, yaw), angular velocity |
+| Altimeter | Altitude, vertical velocity |
+| Velocity | Estimated velocity (x, y, z) |
+| Camera | Downward-facing image |
+| Landing Pad | Relative position when visible |
 
 ## Configuration
 
 Edit `config.py` to customize:
-- Drone/rover starting positions
+- Drone/rover positions
-- Physical properties (mass, size)
 - Controller gains (Kp, Kd)
-- Landing detection thresholds
+- Camera settings
+- Landing thresholds
 
 ## Command Line Options
 
 ```bash
-# Standalone (no ROS 2)
+# Movement patterns
-python standalone_simulation.py --pattern circular --speed 0.3
-
-# PyBullet + ROS 2
-python run_bridge.py --pattern circular --speed 0.3 --host <SIMULATOR_IP>
-
-# Gazebo + ROS 2  
-python run_gazebo.py --pattern circular --speed 0.3
-
-Options:
 --pattern, -p   stationary, linear, circular, square, random
 --speed, -s     Speed in m/s (default: 0.5)
 --amplitude, -a Amplitude in meters (default: 2.0)
-  --host, -H      Simulator IP (default: 0.0.0.0)
 ```
 
-## GPS-Denied Sensors
-
-| Sensor | Data |
-|--------|------|
-| **IMU** | Orientation (roll, pitch, yaw), angular velocity |
-| **Altimeter** | Altitude, vertical velocity |
-| **Velocity** | Estimated horizontal velocity (x, y, z) |
-| **Camera** | 320x240 downward-facing JPEG image |
-| **Landing Pad** | Relative position when visible in camera FOV |
-
 ## Documentation
 
 | Document | Description |
 |----------|-------------|
-| [Installation](docs/installation.md) | Platform setup guides + WSL2 |
+| [Installation](docs/installation.md) | Full setup guide |
-| [Architecture](docs/architecture.md) | System components diagram |
+| [Architecture](docs/architecture.md) | System overview |
-| [Gazebo Guide](docs/gazebo.md) | Gazebo-specific instructions |
+| [ArduPilot](docs/ardupilot.md) | ArduPilot SITL guide |
-| [PyBullet Guide](docs/pybullet.md) | PyBullet-specific instructions |
+| [Gazebo](docs/gazebo.md) | Gazebo guide |
-| [ArduPilot Guide](docs/ardupilot.md) | **ArduPilot SITL + MAVProxy** |
-| [Protocol](docs/protocol.md) | Sensor data formats |
-| [Drone Guide](docs/drone_guide.md) | Landing algorithm guide |
-
-## Network Setup (Remote Simulator)
-
-Run simulator on one machine, controllers on another:
-
-**Machine 1 (with display):**
-```bash
-python simulation_host.py  # Listens on 0.0.0.0:5555
-```
-
-**Machine 2 (headless):**
-```bash
-python run_bridge.py --host 192.168.1.100  # Connect to Machine 1
-```

build_exe.py

@@ -7,6 +7,8 @@ Usage:
   python build_exe.py                    # Build standalone_simulation
   python build_exe.py simulation_host    # Build simulation_host
   python build_exe.py standalone         # Build standalone_simulation
+  python build_exe.py ardupilot          # Build ArduPilot launcher
+  python build_exe.py camera_viewer      # Build camera feed viewer
   python build_exe.py all                # Build all
 """
 
@@ -24,12 +26,25 @@ except ImportError as e:
     print("Install with: pip install pyinstaller pybullet")
     sys.exit(1)
 
+# Check for pymavlink (optional for ArduPilot builds)
+try:
+    from pymavlink import mavutil
+    PYMAVLINK_AVAILABLE = True
+except ImportError:
+    PYMAVLINK_AVAILABLE = False
+
 
 def get_pybullet_data_path() -> str:
     return pybullet_data.getDataPath()
 
 
-def build_executable(source_name: str, output_name: str, console: bool = True):
+def build_executable(
+    source_name: str,
+    output_name: str,
+    console: bool = True,
+    hidden_imports: list = None,
+    collect_data: list = None
+):
     """Build a single executable."""
     script_dir = Path(__file__).parent
     source_file = script_dir / source_name
@@ -59,6 +74,16 @@ def build_executable(source_name: str, output_name: str, console: bool = True):
         f'--add-data={data_spec}',
     ]
     
+    # Add hidden imports if specified
+    if hidden_imports:
+        for imp in hidden_imports:
+            build_args.append(f'--hidden-import={imp}')
+    
+    # Add data collection for packages
+    if collect_data:
+        for pkg in collect_data:
+            build_args.append(f'--collect-data={pkg}')
+    
     if console:
         build_args.append('--console')
     else:
@@ -92,7 +117,7 @@ def main():
         'target',
         nargs='?',
         default='standalone',
-        choices=['standalone', 'simulation_host', 'all'],
+        choices=['standalone', 'simulation_host', 'ardupilot', 'mavlink_bridge', 'camera_viewer', 'all'],
         help='What to build (default: standalone)'
     )
     args = parser.parse_args()
@@ -102,9 +127,11 @@ def main():
     print("=" * 60)
     print(f"Platform: {platform.system()}")
     print(f"PyBullet data: {get_pybullet_data_path()}")
+    print(f"pymavlink: {'Available' if PYMAVLINK_AVAILABLE else 'Not installed'}")
     
     success = True
     
+    # Build standalone simulation
     if args.target in ['standalone', 'all']:
         success &= build_executable(
             'standalone_simulation.py',
@@ -112,6 +139,7 @@ def main():
             console=False
         )
     
+    # Build simulation host
     if args.target in ['simulation_host', 'all']:
         success &= build_executable(
             'simulation_host.py',
@@ -119,11 +147,81 @@ def main():
             console=True
         )
     
+    # Build MAVLink bridge (requires pymavlink)
+    if args.target in ['mavlink_bridge', 'all']:
+        if not PYMAVLINK_AVAILABLE:
+            print("\nWarning: pymavlink not installed, skipping mavlink_bridge build")
+            print("Install with: pip install pymavlink")
+            if args.target == 'mavlink_bridge':
+                success = False
+        else:
+            success &= build_executable(
+                'mavlink_bridge.py',
+                'mavlink_bridge',
+                console=True,
+                hidden_imports=[
+                    'pymavlink',
+                    'pymavlink.mavutil',
+                    'pymavlink.dialects.v20.ardupilotmega',
+                ],
+                collect_data=['pymavlink']
+            )
+    
+    # Build ArduPilot runner (requires pymavlink)
+    if args.target in ['ardupilot', 'all']:
+        if not PYMAVLINK_AVAILABLE:
+            print("\nWarning: pymavlink not installed, skipping ardupilot build")
+            print("Install with: pip install pymavlink")
+            if args.target == 'ardupilot':
+                success = False
+        else:
+            success &= build_executable(
+                'run_ardupilot.py',
+                'run_ardupilot',
+                console=True,
+                hidden_imports=[
+                    'pymavlink',
+                    'pymavlink.mavutil',
+                    'pymavlink.dialects.v20.ardupilotmega',
+                    'mavlink_bridge',
+                    'drone_controller',
+                    'rover_controller',
+                ],
+                collect_data=['pymavlink']
+            )
+    
+    # Build camera viewer (requires opencv)
+    if args.target in ['camera_viewer', 'all']:
+        try:
+            import cv2
+            success &= build_executable(
+                'camera_viewer.py',
+                'camera_viewer',
+                console=True,
+                hidden_imports=[
+                    'cv2',
+                    'numpy',
+                ],
+                collect_data=['cv2']
+            )
+        except ImportError:
+            print("\nWarning: opencv-python not installed, skipping camera_viewer build")
+            print("Install with: pip install opencv-python")
+            if args.target == 'camera_viewer':
+                success = False
+    
     print()
     print("=" * 60)
     if success:
         print("  BUILD COMPLETE!")
         print("  Executables in: dist/")
+        print()
+        print("  Available executables:")
+        dist_dir = Path(__file__).parent / "dist"
+        if dist_dir.exists():
+            for exe in dist_dir.iterdir():
+                if exe.is_file() and not exe.name.startswith('.'):
+                    print(f"    - {exe.name}")
     else:
         print("  BUILD FAILED!")
         sys.exit(1)
@@ -132,3 +230,4 @@ def main():
 
 if __name__ == '__main__':
     main()
+

camera_viewer.py

@@ -0,0 +1,420 @@
+#!/usr/bin/env python3
+"""
+Drone Camera Viewer - Real-time camera feed display.
+
+Displays the drone's camera feed from Gazebo simulation in a separate window.
+Works with ROS 2 image topics.
+
+Usage:
+    python camera_viewer.py                     # Default topic /drone/camera
+    python camera_viewer.py --topic /camera/image_raw
+    python camera_viewer.py --fps 60            # Higher framerate
+    python camera_viewer.py --record output.avi # Record to file
+
+Topics:
+    - /drone/camera (Gazebo simulation)
+    - /camera/image_raw (generic)
+    - /ap/camera (ArduPilot if available)
+"""
+
+import argparse
+import sys
+import time
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+from sensor_msgs.msg import Image
+
+# Try to import OpenCV
+try:
+    import cv2
+    import numpy as np
+    CV2_AVAILABLE = True
+except ImportError:
+    CV2_AVAILABLE = False
+    print("Warning: OpenCV not installed. Install with: pip install opencv-python")
+
+# Try to import cv_bridge (optional, for more robust conversion)
+try:
+    from cv_bridge import CvBridge
+    CV_BRIDGE_AVAILABLE = True
+except ImportError:
+    CV_BRIDGE_AVAILABLE = False
+
+
+class CameraViewer(Node):
+    """ROS 2 node that displays camera feed in a window."""
+    
+    def __init__(
+        self,
+        topic: str = '/drone/camera',
+        fps: int = 30,
+        window_name: str = 'Drone Camera',
+        record_file: Optional[str] = None,
+        show_info: bool = True
+    ):
+        super().__init__('camera_viewer')
+        
+        self._topic = topic
+        self._target_fps = fps
+        self._window_name = window_name
+        self._show_info = show_info
+        self._record_file = record_file
+        
+        # State
+        self._latest_frame: Optional[np.ndarray] = None
+        self._frame_count = 0
+        self._fps_actual = 0.0
+        self._last_fps_time = time.time()
+        self._fps_frame_count = 0
+        self._running = True
+        
+        # Video writer for recording
+        self._video_writer: Optional[cv2.VideoWriter] = None
+        
+        # CV Bridge for image conversion
+        if CV_BRIDGE_AVAILABLE:
+            self._bridge = CvBridge()
+        else:
+            self._bridge = None
+        
+        # QoS for camera topics (best effort for performance)
+        camera_qos = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1
+        )
+        
+        # Subscribe to camera topic
+        self._camera_sub = self.create_subscription(
+            Image,
+            topic,
+            self._camera_callback,
+            camera_qos
+        )
+        
+        self.get_logger().info('=' * 50)
+        self.get_logger().info('Camera Viewer Starting...')
+        self.get_logger().info(f'  Topic: {topic}')
+        self.get_logger().info(f'  Target FPS: {fps}')
+        if record_file:
+            self.get_logger().info(f'  Recording to: {record_file}')
+        self.get_logger().info('=' * 50)
+        self.get_logger().info('Press Q or ESC in the window to quit')
+        
+        # Create display timer at target FPS
+        display_period = 1.0 / fps
+        self._display_timer = self.create_timer(display_period, self._display_frame)
+    
+    def _camera_callback(self, msg: Image):
+        """Process incoming camera image."""
+        try:
+            # Convert ROS Image to OpenCV format
+            if self._bridge and CV_BRIDGE_AVAILABLE:
+                # Use cv_bridge for robust conversion
+                self._latest_frame = self._bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+            else:
+                # Manual conversion
+                self._latest_frame = self._ros_image_to_cv2(msg)
+            
+            self._frame_count += 1
+            self._fps_frame_count += 1
+            
+        except Exception as e:
+            self.get_logger().warning(f'Failed to convert image: {e}')
+    
+    def _ros_image_to_cv2(self, msg: Image) -> np.ndarray:
+        """Convert ROS Image message to OpenCV format."""
+        # Get image dimensions
+        height = msg.height
+        width = msg.width
+        encoding = msg.encoding
+        
+        # Convert based on encoding
+        if encoding in ['rgb8', 'bgr8']:
+            # 3-channel color image
+            channels = 3
+            dtype = np.uint8
+        elif encoding in ['rgba8', 'bgra8']:
+            # 4-channel color image
+            channels = 4
+            dtype = np.uint8
+        elif encoding in ['mono8', '8UC1']:
+            # Grayscale
+            channels = 1
+            dtype = np.uint8
+        elif encoding in ['mono16', '16UC1']:
+            channels = 1
+            dtype = np.uint16
+        elif encoding == '32FC1':
+            channels = 1
+            dtype = np.float32
+        else:
+            # Try to handle as 3-channel uint8
+            channels = 3
+            dtype = np.uint8
+        
+        # Reshape data to image
+        if channels == 1:
+            frame = np.frombuffer(msg.data, dtype=dtype).reshape((height, width))
+            # Convert grayscale to BGR for display
+            frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
+        else:
+            frame = np.frombuffer(msg.data, dtype=dtype).reshape((height, width, channels))
+            
+            # Convert to BGR if needed
+            if encoding == 'rgb8':
+                frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
+            elif encoding == 'rgba8':
+                frame = cv2.cvtColor(frame, cv2.COLOR_RGBA2BGR)
+            elif encoding == 'bgra8':
+                frame = cv2.cvtColor(frame, cv2.COLOR_BGRA2BGR)
+        
+        return frame
+    
+    def _display_frame(self):
+        """Display the latest frame."""
+        if not self._running:
+            return
+        
+        # Calculate actual FPS
+        current_time = time.time()
+        elapsed = current_time - self._last_fps_time
+        if elapsed >= 1.0:
+            self._fps_actual = self._fps_frame_count / elapsed
+            self._fps_frame_count = 0
+            self._last_fps_time = current_time
+        
+        # Check for window close or key press
+        key = cv2.waitKey(1) & 0xFF
+        if key in [ord('q'), ord('Q'), 27]:  # Q or ESC
+            self._running = False
+            self.get_logger().info('Shutting down...')
+            rclpy.shutdown()
+            return
+        
+        # Display frame if available
+        if self._latest_frame is not None:
+            display_frame = self._latest_frame.copy()
+            
+            # Add info overlay
+            if self._show_info:
+                self._add_info_overlay(display_frame)
+            
+            # Show frame
+            cv2.imshow(self._window_name, display_frame)
+            
+            # Record if enabled
+            if self._record_file and self._video_writer is None:
+                # Initialize video writer with frame dimensions
+                h, w = display_frame.shape[:2]
+                fourcc = cv2.VideoWriter_fourcc(*'XVID')
+                self._video_writer = cv2.VideoWriter(
+                    self._record_file, fourcc, self._target_fps, (w, h)
+                )
+            
+            if self._video_writer:
+                self._video_writer.write(display_frame)
+        else:
+            # No frame yet - show waiting message
+            waiting_frame = np.zeros((480, 640, 3), dtype=np.uint8)
+            cv2.putText(
+                waiting_frame,
+                'Waiting for camera feed...',
+                (120, 240),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                1.0,
+                (255, 255, 255),
+                2
+            )
+            cv2.putText(
+                waiting_frame,
+                f'Topic: {self._topic}',
+                (100, 280),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.7,
+                (128, 128, 128),
+                1
+            )
+            cv2.imshow(self._window_name, waiting_frame)
+    
+    def _add_info_overlay(self, frame: np.ndarray):
+        """Add information overlay to frame."""
+        h, w = frame.shape[:2]
+        
+        # Semi-transparent background for text
+        overlay = frame.copy()
+        cv2.rectangle(overlay, (5, 5), (200, 85), (0, 0, 0), -1)
+        cv2.addWeighted(overlay, 0.5, frame, 0.5, 0, frame)
+        
+        # FPS display
+        cv2.putText(
+            frame,
+            f'FPS: {self._fps_actual:.1f}',
+            (10, 25),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.6,
+            (0, 255, 0),
+            2
+        )
+        
+        # Frame count
+        cv2.putText(
+            frame,
+            f'Frame: {self._frame_count}',
+            (10, 50),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.6,
+            (0, 255, 0),
+            2
+        )
+        
+        # Resolution
+        cv2.putText(
+            frame,
+            f'Size: {w}x{h}',
+            (10, 75),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.6,
+            (0, 255, 0),
+            2
+        )
+        
+        # Recording indicator
+        if self._record_file:
+            cv2.circle(frame, (w - 20, 20), 10, (0, 0, 255), -1)
+            cv2.putText(
+                frame,
+                'REC',
+                (w - 60, 25),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.5,
+                (0, 0, 255),
+                2
+            )
+    
+    def shutdown(self):
+        """Clean up resources."""
+        self._running = False
+        if self._video_writer:
+            self._video_writer.release()
+        cv2.destroyAllWindows()
+
+
+def find_camera_topics() -> list:
+    """Find available camera topics."""
+    import subprocess
+    try:
+        result = subprocess.run(
+            ['ros2', 'topic', 'list'],
+            capture_output=True,
+            text=True,
+            timeout=5.0
+        )
+        topics = result.stdout.strip().split('\n')
+        
+        # Filter for likely camera topics
+        camera_keywords = ['camera', 'image', 'rgb', 'depth']
+        camera_topics = [
+            t for t in topics
+            if any(kw in t.lower() for kw in camera_keywords)
+        ]
+        return camera_topics
+    except Exception:
+        return []
+
+
+def main():
+    if not CV2_AVAILABLE:
+        print("ERROR: OpenCV is required for camera viewer")
+        print("Install with: pip install opencv-python")
+        sys.exit(1)
+    
+    parser = argparse.ArgumentParser(
+        description='Drone Camera Viewer - Display camera feed in real-time',
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        epilog="""
+Examples:
+  python camera_viewer.py                        # Default topic
+  python camera_viewer.py --topic /camera/image_raw
+  python camera_viewer.py --fps 60               # Higher framerate
+  python camera_viewer.py --record flight.avi    # Record video
+  python camera_viewer.py --list                 # List camera topics
+
+Common topics:
+  /drone/camera          - Gazebo simulation
+  /camera/image_raw      - Generic camera
+  /camera/color/image_raw - RGB camera
+  /camera/depth/image_raw - Depth camera
+        """
+    )
+    
+    parser.add_argument(
+        '--topic', '-t', type=str, default='/drone/camera',
+        help='Camera image topic (default: /drone/camera)'
+    )
+    parser.add_argument(
+        '--fps', type=int, default=30,
+        help='Target display framerate (default: 30)'
+    )
+    parser.add_argument(
+        '--record', '-r', type=str, default=None,
+        help='Record video to file (e.g., output.avi)'
+    )
+    parser.add_argument(
+        '--no-info', action='store_true',
+        help='Hide info overlay'
+    )
+    parser.add_argument(
+        '--list', '-l', action='store_true',
+        help='List available camera topics and exit'
+    )
+    parser.add_argument(
+        '--window-name', '-w', type=str, default='Drone Camera',
+        help='Window title'
+    )
+    
+    args = parser.parse_args()
+    
+    # List topics and exit
+    if args.list:
+        print("Searching for camera topics...")
+        topics = find_camera_topics()
+        if topics:
+            print("\nAvailable camera topics:")
+            for t in topics:
+                print(f"  {t}")
+        else:
+            print("\nNo camera topics found. Is the simulation running?")
+        return
+    
+    # Initialize ROS 2
+    rclpy.init()
+    
+    viewer = None
+    try:
+        viewer = CameraViewer(
+            topic=args.topic,
+            fps=args.fps,
+            window_name=args.window_name,
+            record_file=args.record,
+            show_info=not args.no_info
+        )
+        
+        rclpy.spin(viewer)
+        
+    except KeyboardInterrupt:
+        print('\nShutting down...')
+    finally:
+        if viewer:
+            viewer.shutdown()
+            viewer.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+        cv2.destroyAllWindows()
+
+
+if __name__ == '__main__':
+    main()

config.py

@@ -47,12 +47,19 @@ ROVER = {
 # =============================================================================
 
 CAMERA = {
+    # Camera sensor settings (for simulation)
     "width": 320,
     "height": 240,
     "fov": 60.0,           # Field of view in degrees
     "near_clip": 0.1,      # Near clipping plane
     "far_clip": 100.0,     # Far clipping plane
     "jpeg_quality": 70,    # JPEG compression quality (1-100)
+    
+    # Camera viewer settings (for camera_viewer.py)
+    "viewer_fps": 30,                      # Display framerate
+    "viewer_topic": "/drone/camera",       # ROS 2 topic for Gazebo
+    "viewer_topic_ardupilot": "/camera/image_raw",  # Topic for ArduPilot
+    "show_info_overlay": True,             # Show FPS and frame count
 }
 
 # =============================================================================
@@ -103,47 +110,57 @@ CONTROLLER = {
 }
 
 # =============================================================================
-# ARDUPILOT / MAVLINK CONFIGURATION
+# ARDUPILOT ROS 2 CONFIGURATION
 # =============================================================================
 
 ARDUPILOT = {
     # Vehicle type
     "vehicle": "ArduCopter",      # ArduCopter, ArduPlane, APMrover2
-    "frame": "gazebo-iris",       # Gazebo model frame
+    "frame": "iris",              # Gazebo model (iris, wildthumper)
     
-    # SITL connection
+    # ROS 2 workspace (set by install_ardupilot.sh)
-    "sitl_host": "127.0.0.1",     # SITL host address
+    "workspace": "~/ardu_ws",
-    "sitl_port": 5760,            # SITL TCP port
     
-    # MAVProxy output (for MAVLink bridge)
+    # Simulation worlds
-    "mavproxy_host": "127.0.0.1",
+    "default_world": "runway",    # runway, maze, sitl
-    "mavproxy_port": 14550,       # MAVProxy UDP output
     
-    # Gazebo plugin connection (JSON interface)
+    # MAVProxy connection (for GCS features)
-    "gazebo_fdm_port_in": 9002,   # Port for motor commands
+    "mavproxy_port": 14550,       # MAVProxy UDP port
-    "gazebo_fdm_port_out": 9003,  # Port for sensor data
     
-    # Arming requirements (for simulation, can be relaxed)
+    # DDS configuration
-    "require_gps": False,         # GPS required for arming
+    "dds_enable": True,           # Enable DDS for native ROS 2 topics
-    "require_ekf": True,          # EKF required for arming
+    "dds_domain_id": 0,           # Must match ROS_DOMAIN_ID
 }
 
+# ArduPilot ROS 2 Topics (read-only reference)
+ARDUPILOT_TOPICS = {
+    # State topics (subscribe)
+    "pose": "/ap/pose/filtered",           # PoseStamped
+    "geopose": "/ap/geopose/filtered",     # GeoPoseStamped
+    "twist": "/ap/twist/filtered",         # TwistStamped
+    "imu": "/ap/imu/filtered",             # Imu
+    "battery": "/ap/battery",              # BatteryState
+    "navsat": "/ap/navsat",                # NavSatFix
+    
+    # Command topics (publish) - via MAVLink
+    "cmd_vel": "/cmd_vel",                 # Twist (velocity commands)
+}
+
+# MAVLink configuration (for pymavlink/mavproxy)
 MAVLINK = {
     # MAVLink system IDs
     "system_id": 1,               # Our system ID
     "component_id": 191,          # MAV_COMP_ID_MISSIONPLANNER
     
-    # Target system (usually the autopilot)
+    # Target system (the autopilot)
     "target_system": 1,
     "target_component": 1,
     
-    # Connection timeout (seconds)
+    # Connection
+    "connection_string": "udpin:127.0.0.1:14550",
+    
+    # Timeouts (seconds)
     "heartbeat_timeout": 5.0,
     "connection_timeout": 30.0,
-    
-    # Data stream rates (Hz)
-    "stream_rate_position": 50,
-    "stream_rate_attitude": 50,
-    "stream_rate_raw_sensors": 50,
-    "stream_rate_extended_status": 5,
 }
+

docs/architecture.md

@@ -1,176 +1,94 @@
 # Architecture Overview
 
-GPS-denied drone landing simulation with multiple operation modes.
-
 ## Operation Modes
 
-### 1. Standalone Mode (Any Platform)
+### 1. Standalone (Any Platform)
 
-Single-process simulation - no ROS 2 or networking required:
+Single process, no ROS 2 required:
+
+```bash
+python standalone_simulation.py --pattern circular
+```
 
 ```
 ┌────────────────────────────────────────┐
 │      standalone_simulation.py          │
 │  ┌──────────────────────────────────┐  │
 │  │  PyBullet Physics + Camera       │  │
-│  │  Built-in Landing Controller     │  │
+│  │  Built-in Controller             │  │
-│  │  Rover Movement Patterns         │  │
+│  │  Rover Movement                  │  │
-│  │  Configuration from config.py    │  │
 │  └──────────────────────────────────┘  │
 └────────────────────────────────────────┘
 ```
 
-### 2. PyBullet + ROS 2 Mode (Two Terminals)
+### 2. Gazebo + ROS 2 (2 Terminals)
+
+**Terminal 1:**
+```bash
+ros2 launch gazebo/launch/drone_landing.launch.py
+```
+
+**Terminal 2:**
+```bash
+python run_gazebo.py --pattern circular
+```
 
 ```
 Terminal 1                    Terminal 2
-┌──────────────────┐         ┌──────────────────────────┐
+┌───────────────────┐        ┌───────────────────┐
-│ simulation_host  │◄─UDP───►│      run_bridge.py       │
+│ Gazebo + Bridge   │◄──────►│ run_gazebo.py     │
-│   (PyBullet)     │         │  ┌────────────────────┐  │
+│ (Physics)         │  ROS   │ + Controllers     │
-│   Port 5555      │         │  │ ROS2SimulatorBridge│  │
+└───────────────────┘        └───────────────────┘
-│                  │         │  │ DroneController    │  │
-│                  │         │  │ RoverController    │  │
-└──────────────────┘         │  └────────────────────┘  │
-                             └──────────────────────────┘
 ```
 
-Data flow:
+### 3. ArduPilot SITL (2 Terminals)
-- RoverController publishes position → Bridge sends to Simulator
-- Simulator moves rover visually AND sends back telemetry
-- DroneController receives telemetry, publishes commands
-- Bridge forwards commands to Simulator
 
-### 3. Gazebo + ROS 2 Mode (Two Terminals, Linux/WSL2)
+**Terminal 1:**
-
+```bash
-```
+ros2 launch ardupilot_gz_bringup iris_runway.launch.py
-Terminal 1                          Terminal 2
-┌───────────────────────────┐      ┌──────────────────────────┐
-│ ros2 launch ... .launch.py│      │      run_gazebo.py       │
-│  ┌─────────────────────┐  │      │  ┌────────────────────┐  │
-│  │ Gazebo (ign gazebo) │  │      │  │ GazeboBridge       │  │
-│  │  - Drone (vel ctrl) │  │◄────►│  │ DroneController    │  │
-│  │  - Rover (vel ctrl) │  │ ROS  │  │ RoverController    │  │
-│  ├─────────────────────┤  │      │  └────────────────────┘  │
-│  │ ros_gz_bridge       │  │      └──────────────────────────┘
-│  └─────────────────────┘  │
-└───────────────────────────┘
 ```
 
-Data flow:
+**Terminal 2:**
-- RoverController publishes to `/rover/cmd_vel` → Gazebo moves rover
+```bash
-- Gazebo publishes odometry → GazeboBridge converts to telemetry
+mavproxy.py --console --map --master=:14550
-- DroneController receives telemetry, publishes to `/cmd_vel`
-- GazeboBridge forwards to `/drone/cmd_vel` → Gazebo moves drone
-
-### 4. ArduPilot SITL + Gazebo Mode (Three Terminals, Linux/WSL2)
-
-```
-Terminal 1           Terminal 2              Terminal 3
-┌──────────────┐    ┌─────────────────┐     ┌────────────────────────┐
-│ Gazebo +     │    │ ArduPilot SITL  │     │   run_ardupilot.py     │
-│ ArduPilot    │◄──►│ sim_vehicle.py  │     │  ┌──────────────────┐  │
-│ Plugin       │JSON│  + MAVProxy     │◄───►│  │ MAVLinkBridge    │  │
-│              │    │                 │ UDP │  │ DroneController  │  │
-│ ardupilot_   │    │ Flight Control  │     │  │ RoverController  │  │
-│ drone.sdf    │    │ + GCS           │     │  └──────────────────┘  │
-└──────────────┘    └─────────────────┘     └────────────────────────┘
 ```
 
-Data flow:
+```
-- ArduPilot SITL sends motor commands → Gazebo plugin controls drone
+┌─────────────────────────────────────────────┐
-- Gazebo plugin sends sensor data → ArduPilot SITL for state estimation
+│        Single Launch Command                │
-- MAVProxy outputs telemetry → MAVLinkBridge converts to ROS telemetry
+│   (Starts SITL + Gazebo + RViz)            │
-- DroneController receives telemetry, publishes velocity commands
+├─────────────────────────────────────────────┤
-- MAVLinkBridge sends MAVLink commands → ArduPilot SITL executes
+│  ArduPilot SITL ◄──► Gazebo ◄──► ROS 2     │
+│       ▲                                     │
+│       │ /ap/* topics                        │
+│       ▼                                     │
+│  MAVProxy (GCS)                            │
+└─────────────────────────────────────────────┘
+```
 
-Key differences from simple Gazebo mode:
+## Key Components
-- Full ArduPilot flight controller (EKF, stabilization, failsafes)
-- Real MAVLink protocol for commands and telemetry
-- Support for all ArduPilot flight modes (GUIDED, LAND, etc.)
-- Arming checks and safety features
-- Compatible with ground control stations (QGroundControl, Mission Planner)
 
-## Components
+| Component | Description |
-
+|-----------|-------------|
-| File | Description |
+| `drone_controller.py` | Your landing algorithm |
-|------|-------------|
+| `gazebo_bridge.py` | Gazebo ↔ ROS bridge |
-| `config.py` | Central configuration (positions, physics, gains) |
+| `mavlink_bridge.py` | MAVLink commands |
-| `standalone_simulation.py` | All-in-one simulation |
+| `camera_viewer.py` | Camera display |
-| `simulation_host.py` | PyBullet physics server (UDP) |
-| `run_bridge.py` | PyBullet bridge + controllers |
-| `run_gazebo.py` | Gazebo bridge + controllers |
-| `run_ardupilot.py` | **ArduPilot SITL** + MAVLink bridge |
-| `mavlink_bridge.py` | MAVLink ↔ ROS 2 bridge |
-| `drone_controller.py` | **Your landing algorithm** |
-| `rover_controller.py` | Moving landing pad |
-| `ros_bridge.py` | ROS-UDP bridge (used by run_bridge.py) |
-| `gazebo_bridge.py` | Gazebo-ROS bridge (used by run_gazebo.py) |
-| `gazebo/launch/drone_landing.launch.py` | ROS 2 launch file for Gazebo |
-| `gazebo/launch/ardupilot_drone.launch.py` | ROS 2 launch file for ArduPilot |
-| `gazebo/worlds/drone_landing.sdf` | Gazebo world with simple velocity control |
-| `gazebo/worlds/ardupilot_drone.sdf` | Gazebo world with ArduPilot plugin |
 
 ## ROS 2 Topics
 
-| Topic | Type | Description |
+| Topic | Direction | Description |
-|-------|------|-------------|
+|-------|-----------|-------------|
-| `/cmd_vel` | `Twist` | Drone commands from DroneController |
+| `/drone/telemetry` | ← | Sensor data (JSON) |
-| `/drone/cmd_vel` | `Twist` | Drone commands to Gazebo |
+| `/cmd_vel` | → | Velocity commands |
-| `/drone/telemetry` | `String` | GPS-denied sensor data (JSON) |
+| `/drone/camera` | ← | Camera images |
-| `/rover/cmd_vel` | `Twist` | Rover velocity to simulator |
+| `/rover/telemetry` | ← | Landing pad position |
-| `/rover/telemetry` | `String` | Rover position (JSON) |
 
-## Network Configuration
+## ArduPilot Topics
 
-All components default to `0.0.0.0` for network accessibility.
+| Topic | Type |
-
+|-------|------|
-### Remote Setup (PyBullet mode)
+| `/ap/pose/filtered` | Position |
-
+| `/ap/twist/filtered` | Velocity |
-**Machine 1 (with display):**
+| `/ap/imu/filtered` | IMU |
-```bash
+| `/ap/battery` | Battery |
-python simulation_host.py  # Listens on 0.0.0.0:5555
-```
-
-**Machine 2 (headless controller):**
-```bash
-python run_bridge.py --host 192.168.1.100
-```
-
-### UDP Ports
-
-| Port | Direction | Content |
-|------|-----------|---------|
-| 5555 | Bridge → Simulator | Commands (JSON) |
-| 5556 | Simulator → Bridge | Telemetry (JSON) |
-
-## GPS-Denied Sensors
-
-All modes provide the same sensor data:
-
-| Sensor | Data |
-|--------|------|
-| **IMU** | Orientation (roll, pitch, yaw), angular velocity |
-| **Altimeter** | Altitude above ground, vertical velocity |
-| **Velocity** | Estimated velocity (x, y, z) |
-| **Camera** | 320x240 downward JPEG (base64) |
-| **Landing Pad** | Relative position (x, y, distance) when visible |
-
-## Configuration (config.py)
-
-| Section | Parameters |
-|---------|------------|
-| `DRONE` | mass, size, color, start_position, thrust/torque scales |
-| `ROVER` | size, color, start_position, default_pattern, default_speed |
-| `CAMERA` | width, height, fov, jpeg_quality |
-| `PHYSICS` | gravity, timestep, telemetry_rate |
-| `CONTROLLER` | Kp_z, Kd_z, Kp_xy, Kd_xy, rate |
-| `LANDING` | success_distance, success_velocity, height_threshold |
-| `NETWORK` | host, command_port, telemetry_port |
-
-## Platform Support
-
-| Mode | Ubuntu | Arch | macOS | Windows | WSL2 |
-|------|--------|------|-------|---------|------|
-| Standalone | ✅ | ✅ | ✅ | ✅ | ✅ |
-| PyBullet+ROS | ✅ | ⚠️ | ❌ | ❌ | ✅ |
-| Gazebo+ROS | ✅ | ⚠️ | ❌ | ❌ | ✅ |

docs/ardupilot.md

@@ -1,285 +1,101 @@
-# ArduPilot SITL + Gazebo Integration
+# ArduPilot SITL Integration
 
-This guide explains how to run the drone simulation with ArduPilot Software-In-The-Loop (SITL) and MAVProxy, providing a realistic flight controller stack.
+Run the simulation with a realistic ArduPilot flight controller.
-
-## Overview
-
-The ArduPilot integration replaces the simple velocity control with a full ArduPilot flight stack:
-
-```
-┌──────────────────┐     ┌─────────────────┐     ┌──────────────────┐
-│  ArduPilot SITL  │◄───►│  Gazebo + Plugin │◄───►│  MAVLink Bridge  │
-│  (Flight Control)│ JSON│  (Physics Sim)   │ ROS │  + Controllers   │
-└──────────────────┘     └─────────────────┘     └──────────────────┘
-        ▲                                                    │
-        │ UDP                                               │
-        │                                                    ▼
-┌──────────────────┐                              ┌──────────────────┐
-│    MAVProxy      │◄────────────────────────────►│ DroneController  │
-│    (GCS)         │     MAVLink Commands         │ (Your Algorithm) │
-└──────────────────┘                              └──────────────────┘
-```
-
-## Components
-
-| Component | Description |
-|-----------|-------------|
-| **ArduPilot SITL** | Full autopilot firmware running in simulation |
-| **ardupilot_gazebo** | Plugin connecting Gazebo physics to ArduPilot |
-| **MAVProxy** | Ground Control Station for monitoring/commands |
-| **MAVLink Bridge** | ROS 2 node bridging MAVLink ↔ ROS topics |
-| **Drone Controller** | Your landing algorithm |
-
-## Prerequisites
-
-### 1. ArduPilot SITL
-
-Install ArduPilot development environment:
-
-```bash
-# Ubuntu/Debian
-git clone https://github.com/ArduPilot/ardupilot.git ~/ardupilot
-cd ~/ardupilot
-git submodule update --init --recursive
-Tools/environment_install/install-prereqs-ubuntu.sh -y
-. ~/.profile
-
-# Set environment
-echo 'export PATH=$PATH:$HOME/ardupilot/Tools/autotest' >> ~/.bashrc
-echo 'export ARDUPILOT_HOME=$HOME/ardupilot' >> ~/.bashrc
-source ~/.bashrc
-```
-
-### 2. ArduPilot Gazebo Plugin
-
-Install the ardupilot_gazebo plugin:
-
-```bash
-# For Gazebo Garden/Harmonic
-git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo
-cd ~/ardupilot_gazebo
-mkdir build && cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j4
-
-# Add to Gazebo plugin path
-echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH' >> ~/.bashrc
-echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH' >> ~/.bashrc
-source ~/.bashrc
-```
-
-### 3. pymavlink
-
-```bash
-pip install pymavlink
-```
 
 ## Quick Start
 
-### Option 1: Integrated Launch (Recommended)
+**Terminal 1 - Simulation:**
-
-This starts everything together:
-
 ```bash
-# Terminal 1: Start Gazebo
+source ~/ardu_ws/install/setup.bash
-ros2 launch gazebo/launch/ardupilot_drone.launch.py
+ros2 launch ardupilot_gz_bringup iris_runway.launch.py
-
-# Terminal 2: Start SITL
-cd ~/ardupilot
-sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map
-
-# Terminal 3: Run bridge + controllers
-python run_ardupilot.py --no-sitl --pattern circular
 ```
 
-### Option 2: Manual Setup
+**Terminal 2 - Control:**
-
 ```bash
-# Terminal 1: Start Gazebo world
+mavproxy.py --console --map --master=:14550
-gz sim -r gazebo/worlds/ardupilot_drone.sdf
-
-# Terminal 2: Start ArduPilot SITL
-cd ~/ardupilot
-sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
-
-# Terminal 3: Run MAVLink bridge + controllers
-python run_ardupilot.py --no-sitl
 ```
 
-### Option 3: Full Automatic
+## Installation
 
 ```bash
-# Starts everything (requires SITL installed)
+./setup/install_ardupilot.sh
-python run_ardupilot.py --pattern circular --console --map
+source ~/.bashrc
 ```
 
-## Flight Operations
+This installs:
+- ArduPilot SITL with DDS
+- Gazebo with ardupilot_gz
+- MAVProxy
 
-### Using MAVProxy Commands
+## MAVProxy Commands
-
-Once connected, use MAVProxy to control the drone:
 
 ```bash
-# Set GUIDED mode for algorithm control
+# Set mode
 mode guided
 
-# Arm motors
+# Arm
 arm throttle
 
-# Take off to 5 meters
+# Takeoff
 takeoff 5
 
 # Land
 mode land
-
-# Disarm
-disarm
 ```
 
-### Using the MAVLink Bridge API
+## ROS 2 Topics
 
-From Python, you can control the drone directly:
+ArduPilot publishes native ROS 2 topics:
 
-```python
+```bash
-from mavlink_bridge import MAVLinkBridge
+# List topics
+ros2 topic list
 
-# Create bridge
+# View position
-bridge = MAVLinkBridge(sitl_port=14550)
+ros2 topic echo /ap/geopose/filtered
 
-# Arm and takeoff
+# View battery
-bridge.set_mode('GUIDED')
+ros2 topic echo /ap/battery
-bridge.arm()
-bridge.takeoff(altitude=5.0)
-
-# Land
-bridge.land()
 ```
 
-## Files
+| Topic | Type |
+|-------|------|
+| `/ap/pose/filtered` | PoseStamped |
+| `/ap/twist/filtered` | TwistStamped |
+| `/ap/imu/filtered` | Imu |
+| `/ap/battery` | BatteryState |
 
-| File | Description |
+## Available Worlds
-|------|-------------|
-| `mavlink_bridge.py` | ROS 2 ↔ MAVLink bridge |
-| `run_ardupilot.py` | Integrated launcher |
-| `gazebo/worlds/ardupilot_drone.sdf` | Gazebo world with ArduPilot plugin |
-| `gazebo/launch/ardupilot_drone.launch.py` | ROS 2 launch file |
 
-## Configuration
+```bash
+# Iris on runway
+ros2 launch ardupilot_gz_bringup iris_runway.launch.py
 
-Edit `config.py` to adjust ArduPilot settings:
+# Iris in maze
+ros2 launch ardupilot_gz_bringup iris_maze.launch.py
 
-```python
+# Rover
-ARDUPILOT = {
+ros2 launch ardupilot_gz_bringup wildthumper_playpen.launch.py
-    "vehicle": "ArduCopter",      # ArduCopter, ArduPlane, APMrover2
-    "frame": "gazebo-iris",       # Gazebo frame
-    "sitl_host": "127.0.0.1",
-    "sitl_port": 5760,
-    "mavproxy_port": 14550,
-}
-
-MAVLINK = {
-    "system_id": 1,
-    "component_id": 191,
-    "heartbeat_timeout": 5.0,
-}
 ```
 
-## Telemetry Format
+## Using the Launcher
 
-The MAVLink bridge publishes telemetry in the same format as other modes:
+```bash
-
+python run_ardupilot.py --world runway
-```json
+python run_ardupilot.py --world maze
-{
+python run_ardupilot.py --vehicle rover
-    "imu": {
-        "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0},
-        "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}
-    },
-    "altimeter": {
-        "altitude": 5.0,
-        "vertical_velocity": 0.0
-    },
-    "velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
-    "position": {"x": 0.0, "y": 0.0, "z": 5.0},
-    "landing_pad": {
-        "relative_x": 0.5,
-        "relative_y": 0.2,
-        "distance": 4.8,
-        "confidence": 0.95
-    },
-    "battery": {"voltage": 12.6, "remaining": 100},
-    "armed": true,
-    "flight_mode": "GUIDED",
-    "connected": true
-}
 ```
 
 ## Troubleshooting
 
-### SITL Not Starting
+**No ROS 2 topics:**
-
 ```bash
-# Check if SITL is installed
+# Check DDS is enabled
-which sim_vehicle.py
+param set DDS_ENABLE 1
-
-# Set ArduPilot path
-export ARDUPILOT_HOME=~/ardupilot
-export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest
 ```
 
-### Gazebo Plugin Not Found
+**Can't arm:**
-
 ```bash
-# Check plugin path
+# Disable pre-arm checks (simulation only)
-echo $GZ_SIM_SYSTEM_PLUGIN_PATH
-
-# Verify plugin exists
-ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
-```
-
-### No MAVLink Connection
-
-```bash
-# Check if SITL is listening
-netstat -tuln | grep 14550
-
-# Test with mavlink console
-python -c "from pymavlink import mavutil; c = mavutil.mavlink_connection('udpin:127.0.0.1:14550'); print(c.wait_heartbeat())"
-```
-
-### Drone Won't Arm
-
-Common issues:
-1. **Pre-arm checks failing** - Check MAVProxy console for errors
-2. **GPS required** - In simulation, you may need to wait for GPS lock
-3. **EKF not ready** - Wait for EKF to initialize
-
-Disable pre-arm checks for testing (not recommended for real flights):
-```
-# In MAVProxy
 param set ARMING_CHECK 0
 ```
-
-## Flight Modes
-
-| Mode | Description |
-|------|-------------|
-| **GUIDED** | Accept velocity/position commands from controller |
-| **LOITER** | Hold position (GPS required) |
-| **ALT_HOLD** | Maintain altitude, manual horizontal |
-| **LAND** | Automatic landing |
-| **STABILIZE** | Attitude stabilization only |
-
-For autonomous landing, use **GUIDED** mode.
-
-## Architecture Comparison
-
-| Feature | Simple Gazebo | ArduPilot + Gazebo |
-|---------|--------------|-------------------|
-| Flight Controller | Velocity control | Full ArduPilot |
-| Stabilization | Manual PD | Inbuilt EKF + PID |
-| Flight Modes | None | All ArduPilot modes |
-| Arming | Not required | Safety checks |
-| Failsafes | None | Battery, GPS, etc. |
-| MAVLink | No | Full protocol |
-| GCS Support | No | QGC, Mission Planner |
-| Realism | Low | High |

docs/drone_guide.md

@@ -1,28 +1,23 @@
-# DroneController Guide (GPS-Denied)
+# DroneController Guide
 
-Implement your landing algorithm in `drone_controller.py`.
+Implement your GPS-denied landing algorithm in `drone_controller.py`.
 
 ## Quick Start
 
 1. Edit `drone_controller.py`
 2. Find `calculate_landing_maneuver()`
 3. Implement your algorithm
-4. Test with any mode:
+4. Test: `python standalone_simulation.py`
-   - `python standalone_simulation.py --pattern stationary` (standalone)
-   - `python run_bridge.py --pattern stationary` (PyBullet + ROS 2)
-   - `python run_gazebo.py --pattern stationary` (Gazebo + ROS 2)
 
-## GPS-Denied Challenge
+## Sensors Available
 
-No GPS available. You must use:
+| Sensor | Description |
-
+|--------|-------------|
-| Sensor | Data |
+| IMU | Orientation, angular velocity |
-|--------|------|
+| Altimeter | Altitude, vertical velocity |
-| **IMU** | Orientation, angular velocity |
+| Velocity | Estimated velocity (x, y, z) |
-| **Altimeter** | Altitude, vertical velocity |
+| Camera | 320x240 downward image |
-| **Velocity** | Estimated from optical flow |
+| Landing Pad | Relative position (may be null!) |
-| **Camera** | 320x240 downward image (base64 JPEG) |
-| **Landing Pad** | Relative position (may be null!) |
 
 ## Function to Implement
 
@@ -34,55 +29,20 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry):
 
 ## Sensor Data
 
-### IMU
 ```python
-imu = telemetry['imu']
+# Altitude
-roll = imu['orientation']['roll']
+altitude = telemetry['altimeter']['altitude']
-pitch = imu['orientation']['pitch']
+vertical_vel = telemetry['altimeter']['vertical_velocity']
-yaw = imu['orientation']['yaw']
-angular_vel = imu['angular_velocity']  # {x, y, z}
-```
 
-### Altimeter
+# Velocity
-```python
+vel_x = telemetry['velocity']['x']
-altimeter = telemetry['altimeter']
+vel_y = telemetry['velocity']['y']
-altitude = altimeter['altitude']
-vertical_vel = altimeter['vertical_velocity']
-```
 
-### Velocity
+# Landing Pad (may be None!)
-```python
+landing_pad = telemetry.get('landing_pad')
-velocity = telemetry['velocity']  # {x, y, z} in m/s
+if landing_pad:
-```
+    relative_x = landing_pad['relative_x']
-
+    relative_y = landing_pad['relative_y']
-### Camera
-The drone has a downward-facing camera providing 320x240 JPEG images.
-
-```python
-import base64
-from PIL import Image
-import io
-
-camera = telemetry['camera']
-image_b64 = camera.get('image')
-
-if image_b64:
-    image_bytes = base64.b64decode(image_b64)
-    image = Image.open(io.BytesIO(image_bytes))
-    # Process image for custom vision algorithms
-```
-
-### Landing Pad (Vision)
-**Important: May be None if pad not visible!**
-
-```python
-landing_pad = telemetry['landing_pad']
-
-if landing_pad is not None:
-    relative_x = landing_pad['relative_x']   # body frame
-    relative_y = landing_pad['relative_y']   # body frame
-    distance = landing_pad['distance']       # vertical
-    confidence = landing_pad['confidence']   # 0-1
 ```
 
 ## Control Output
@@ -112,7 +72,7 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry):
     thrust = 0.5 * (0 - altitude) - 0.3 * vertical_vel
     
     # Horizontal control
-    if landing_pad is not None:
+    if landing_pad:
         pitch = 0.3 * landing_pad['relative_x'] - 0.2 * vel_x
         roll = 0.3 * landing_pad['relative_y'] - 0.2 * vel_y
     else:
@@ -124,84 +84,43 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry):
 
 ## Using the Camera
 
-You can implement custom vision processing on the camera image:
-
 ```python
 import cv2
 import numpy as np
 import base64
 
-def process_camera(telemetry):
 camera = telemetry.get('camera', {})
 image_b64 = camera.get('image')
 
-    if not image_b64:
+if image_b64:
-        return None
-    
-    # Decode JPEG
     image_bytes = base64.b64decode(image_b64)
     nparr = np.frombuffer(image_bytes, np.uint8)
     image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
-    
+    # Process image...
-    # Example: detect green landing pad
-    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
-    green_mask = cv2.inRange(hsv, (35, 50, 50), (85, 255, 255))
-    
-    # Find contours
-    contours, _ = cv2.findContours(green_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
-    
-    if contours:
-        largest = max(contours, key=cv2.contourArea)
-        M = cv2.moments(largest)
-        if M['m00'] > 0:
-            cx = int(M['m10'] / M['m00'])
-            cy = int(M['m01'] / M['m00'])
-            # cx, cy is center of detected pad in image coordinates
-            return (cx, cy)
-    
-    return None
 ```
 
-## Strategies
-
-### When Pad Not Visible
-- Maintain altitude and stabilize
-- Search by ascending or spiraling
-- Dead reckoning from last known position
-
-### State Machine
-1. Search → find pad
-2. Approach → move above pad
-3. Align → center over pad
-4. Descend → controlled descent
-5. Land → touch down
-
 ## Testing
 
 ```bash
-# Easy - stationary rover
+# Easy - stationary
 python standalone_simulation.py --pattern stationary
 
-# Medium - slow circular movement
+# Medium - circular
-python standalone_simulation.py --pattern circular --speed 0.2
+python standalone_simulation.py --pattern circular --speed 0.3
 
-# Hard - faster random movement
+# Hard - random
-python standalone_simulation.py --pattern random --speed 0.3
+python standalone_simulation.py --pattern random --speed 0.5
-
-# With ROS 2 (Gazebo)
-ros2 launch gazebo/launch/drone_landing.launch.py  # Terminal 1
-python run_gazebo.py --pattern circular            # Terminal 2
 ```
 
 ## Configuration
 
-Edit `config.py` to tune controller gains:
+Edit `config.py`:
 
 ```python
 CONTROLLER = {
-    "Kp_z": 0.5,    # Altitude proportional gain
+    "Kp_z": 0.5,    # Altitude proportional
-    "Kd_z": 0.3,    # Altitude derivative gain
+    "Kd_z": 0.3,    # Altitude derivative
-    "Kp_xy": 0.3,   # Horizontal proportional gain
+    "Kp_xy": 0.3,   # Horizontal proportional
-    "Kd_xy": 0.2,   # Horizontal derivative gain
+    "Kd_xy": 0.2,   # Horizontal derivative
 }
 ```

docs/gazebo.md

@@ -1,159 +1,72 @@
 # Gazebo Simulation Guide
 
-Running the GPS-denied drone simulation with Gazebo Ignition Fortress on Linux/WSL2.
+## Quick Start (2 Terminals)
 
-## Quick Start (Two Terminals)
+**Terminal 1 - Gazebo:**
-
-**Terminal 1 - Launch Gazebo + Bridge:**
 ```bash
-source activate.sh
 ros2 launch gazebo/launch/drone_landing.launch.py
 ```
 
-**Terminal 2 - Run Controllers:**
+**Terminal 2 - Controllers:**
 ```bash
 source activate.sh
-python run_gazebo.py --pattern circular --speed 0.3
+python run_gazebo.py --pattern circular
 ```
 
-Both the drone AND rover will move!
-
 ## Command Options
 
 ```bash
-python run_gazebo.py --help
+python run_gazebo.py --pattern circular --speed 0.5
 
 Options:
-  --pattern       stationary, linear, circular, square, random
+  --pattern, -p   stationary, linear, circular, square, random
   --speed, -s     Rover speed in m/s (default: 0.5)
-  --amplitude, -a Movement amplitude (default: 2.0)
+  --amplitude, -a Movement radius (default: 2.0)
   --no-rover      Disable rover controller
 ```
 
-## How It Works
+## View Camera
-
-1. **Gazebo** runs the physics simulation with:
-   - Drone with `VelocityControl` plugin (responds to `/drone/cmd_vel`)
-   - Rover with `VelocityControl` plugin (responds to `/rover/cmd_vel`)
-
-2. **ros_gz_bridge** connects Gazebo topics to ROS 2
-
-3. **run_gazebo.py** starts:
-   - `GazeboBridge` - converts ROS topics to telemetry format
-   - `DroneController` - your landing algorithm
-   - `RoverController` - moves the landing pad
-
-## GPS-Denied Sensors
-
-The `GazeboBridge` provides the same sensor interface as PyBullet:
-
-| Sensor | Source |
-|--------|--------|
-| IMU | Gazebo odometry orientation |
-| Altimeter | Gazebo Z position |
-| Velocity | Gazebo twist |
-| Camera | Gazebo camera sensor (if enabled) |
-| Landing Pad | Computed from relative position |
-
-## Topics
-
-### ROS 2 Topics (your code uses these)
-
-| Topic | Type | Direction |
-|-------|------|-----------|
-| `/cmd_vel` | `Twist` | Input (from DroneController) |
-| `/drone/telemetry` | `String` | Output (to DroneController) |
-| `/rover/telemetry` | `String` | Output (rover position) |
-
-### Gazebo Topics (bridged automatically)
-
-| Topic | Type | Description |
-|-------|------|-------------|
-| `/drone/cmd_vel` | `Twist` | Drone velocity commands |
-| `/rover/cmd_vel` | `Twist` | Rover velocity commands |
-| `/model/drone/odometry` | `Odometry` | Drone state |
-| `/drone/imu` | `IMU` | IMU sensor data |
-| `/clock` | `Clock` | Simulation time |
-
-## Headless Mode (WSL2 / No GPU)
-
-Run Gazebo without GUI:
 
 ```bash
-# Server mode only
+python camera_viewer.py --topic /drone/camera
+```
+
+## Sensors
+
+| Sensor | Description |
+|--------|-------------|
+| IMU | Orientation, angular velocity |
+| Altimeter | Altitude, vertical velocity |
+| Velocity | Estimated velocity (x, y, z) |
+| Camera | Downward-facing image |
+| Landing Pad | Relative position |
+
+## ROS 2 Topics
+
+| Topic | Direction |
+|-------|-----------|
+| `/cmd_vel` | Your commands → Drone |
+| `/drone/telemetry` | Sensors → You |
+| `/drone/camera` | Camera → You |
+| `/rover/telemetry` | Rover position |
+
+## Headless Mode (WSL2)
+
+```bash
+# Server only (no GUI)
 ign gazebo -s gazebo/worlds/drone_landing.sdf
 ```
 
-Then run the bridge manually:
-```bash
-ros2 run ros_gz_bridge parameter_bridge \
-  /drone/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist \
-  /rover/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist \
-  /model/drone/odometry@nav_msgs/msg/Odometry[ignition.msgs.Odometry
-```
-
-## World File Details
-
-The world file `gazebo/worlds/drone_landing.sdf` includes:
-
-- **Drone** at (0, 0, 2) with:
-  - `VelocityControl` plugin for movement
-  - `OdometryPublisher` plugin for telemetry
-  - IMU sensor
-
-- **Landing Pad (Rover)** at (0, 0, 0.15) with:
-  - `VelocityControl` plugin for movement
-  - Visual H marker
-
 ## Troubleshooting
 
-### Drone falls immediately
+**Drone falls:**
+- Check `run_gazebo.py` is running
+- Check topic: `ros2 topic echo /drone/cmd_vel`
 
-The drone should hover with the controller running. If it falls:
+**Rover doesn't move:**
-1. Check that `run_gazebo.py` is running
+- Check topic: `ros2 topic echo /rover/cmd_vel`
-2. Verify the bridge shows "Passing message from ROS"
-3. Check `/drone/cmd_vel` topic: `ros2 topic echo /drone/cmd_vel`
 
-### Rover doesn't move
+**Model not found:**
-
-1. Check that `/rover/cmd_vel` is bridged
-2. Verify RoverController is publishing: `ros2 topic echo /rover/cmd_vel`
-
-### Model not found
-
-Set the model path:
 ```bash
 export GZ_SIM_RESOURCE_PATH=$PWD/gazebo/models:$GZ_SIM_RESOURCE_PATH
-export IGN_GAZEBO_RESOURCE_PATH=$PWD/gazebo/models:$IGN_GAZEBO_RESOURCE_PATH
 ```
-
-### "Cannot connect to display" (WSL2)
-
-Use headless mode:
-```bash
-ign gazebo -s gazebo/worlds/drone_landing.sdf
-```
-
-Or ensure WSLg is working:
-```bash
-export DISPLAY=:0
-```
-
-### Plugin not found
-
-For Ignition Fortress, plugins use `libignition-gazebo-*-system.so` naming.
-
-Check available plugins:
-```bash
-ls /usr/lib/x86_64-linux-gnu/ign-gazebo-6/plugins/
-```
-
-## Launch File Options
-
-```bash
-ros2 launch gazebo/launch/drone_landing.launch.py use_sim_time:=true
-```
-
-| Argument | Default | Description |
-|----------|---------|-------------|
-| `use_sim_time` | `true` | Use Gazebo clock |

docs/installation.md

@@ -1,503 +1,125 @@
 # Installation Guide
 
-Setup instructions for all supported platforms.
-
 ## Quick Install
 
+```bash
+# Ubuntu/Debian
+./setup/install_ubuntu.sh
+source activate.sh
+
+# Test
+python standalone_simulation.py
+```
+
+## Install Scripts
+
 | Platform | Command |
 |----------|---------|
 | Ubuntu/Debian | `./setup/install_ubuntu.sh` |
-| Ubuntu + ArduPilot | `./setup/install_ubuntu.sh --with-ardupilot` |
+| ArduPilot SITL | `./setup/install_ardupilot.sh` |
 | Arch Linux | `./setup/install_arch.sh` |
 | macOS | `./setup/install_macos.sh` |
 | Windows | `.\setup\install_windows.ps1` |
 
-After installation:
+## Platform Support
-```bash
-source activate.sh          # Linux/macOS
-. .\activate.ps1             # Windows PowerShell
 
-python standalone_simulation.py
+| Mode | Ubuntu | macOS | Windows |
-```
+|------|--------|-------|---------|
-
+| Standalone | ✅ | ✅ | ✅ |
----
+| Gazebo + ROS 2 | ✅ | ❌ | WSL2 |
-
+| ArduPilot SITL | ✅ | ❌ | WSL2 |
-## Platform Compatibility
-
-| Feature | Ubuntu | Arch | macOS | Windows | WSL2 |
-|---------|--------|------|-------|---------|------|
-| **Standalone Simulation** | ✅ | ✅ | ✅ | ✅ | ✅ |
-| **ROS 2** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ |
-| **Gazebo** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ |
-| **ArduPilot SITL** | ✅ | ⚠️ Manual | ❌ | ❌ | ✅ |
-| **Full Mode** | ✅ | ⚠️ | ❌ | ❌ | ✅ |
-| **GUI Support** | ✅ | ✅ | ✅ | ✅ | ✅ WSLg |
-
-**Legend:**
-- ✅ Fully supported
-- ⚠️ Available but requires extra setup
-- ❌ Not supported
-
-**Recommendation for Windows users:** Use WSL2 for the full experience (ROS 2 + Gazebo).
 
 ---
 
 ## Ubuntu/Debian
 
-**Tested on:** Ubuntu 22.04 (Jammy), Ubuntu 24.04 (Noble)
-
 ```bash
-# Run installer
 ./setup/install_ubuntu.sh
-
-# Activate environment
 source activate.sh
-
-# Run simulation
 python standalone_simulation.py
 ```
 
 **Installs:**
-- ROS 2 (Humble or Jazzy based on Ubuntu version)
+- ROS 2 Humble/Jazzy
-- Gazebo (ros-gz)
+- Gazebo
-- Python packages: pybullet, numpy, pillow, pyinstaller, pymavlink
+- Python packages (pybullet, numpy, opencv, pymavlink)
+
+---
+
+## ArduPilot SITL
+
+For realistic flight controller simulation:
 
-**With ArduPilot SITL (full flight controller):**
 ```bash
-# Run installer with ArduPilot
+./setup/install_ardupilot.sh
-./setup/install_ubuntu.sh --with-ardupilot
+source ~/.bashrc
+```
 
-# This will also install:
+**Installs:**
-# - ArduPilot SITL (~15-20 min build)
+- ArduPilot SITL
-# - ArduPilot Gazebo plugin
+- ardupilot_gz (Gazebo integration)
-# - MAVProxy
+- MAVProxy
+
+**Run:**
+```bash
+# Terminal 1
+source ~/ardu_ws/install/setup.bash
+ros2 launch ardupilot_gz_bringup iris_runway.launch.py
+
+# Terminal 2
+mavproxy.py --console --map --master=:14550
 ```
 
 ---
 
-## Arch Linux
+## Windows (WSL2)
 
-**Tested on:** Arch Linux (rolling release)
+1. Install WSL2:
-
+```powershell
-```bash
+wsl --install -d Ubuntu-22.04
-# Run installer
-./setup/install_arch.sh
-
-# Activate environment
-source activate.sh
-
-# Run simulation
-python standalone_simulation.py
 ```
 
-**Installs:**
+2. Open Ubuntu and run:
-- Python packages: pybullet, numpy, pillow, pyinstaller
-- yay (AUR helper)
-
-**Optional ROS 2 (from AUR):**
 ```bash
-yay -S ros-humble-desktop
+./setup/install_ubuntu.sh
-yay -S ros-humble-ros-gz
+source activate.sh
+python standalone_simulation.py
 ```
 
 ---
 
 ## macOS
 
-**Tested on:** macOS 12+ (Monterey, Ventura, Sonoma)
-
 ```bash
-# Run installer
 ./setup/install_macos.sh
-
-# Activate environment
 source activate.sh
-
-# Run simulation
 python standalone_simulation.py
 ```
 
-**Installs:**
+**Note:** ROS 2 and Gazebo not supported on macOS. Use standalone mode.
-- Homebrew (if not present)
-- Python 3.11
-- Python packages: pybullet, numpy, pillow, pyinstaller
-
-**Note:** ROS 2 and Gazebo are not supported on macOS. Use standalone mode.
 
 ---
 
-## Windows
+## Manual Install
-
-**Tested on:** Windows 10, Windows 11
-
-```powershell
-# Open PowerShell as Administrator
-Set-ExecutionPolicy RemoteSigned -Scope CurrentUser
-
-# Run installer
-.\setup\install_windows.ps1
-
-# Activate environment
-. .\activate.ps1
-
-# Run simulation
-python standalone_simulation.py
-```
-
-**Installs:**
-- Chocolatey (package manager)
-- Python 3.11
-- Python packages: pybullet, numpy, pillow, pyinstaller
-
-**Note:** ROS 2 and Gazebo are not supported natively on Windows. Use standalone mode or WSL2 (below).
-
----
-
-## Windows with WSL2 (Full Linux Experience)
-
-WSL2 lets you run full Linux on Windows with GUI support. This enables ROS 2 and Gazebo!
-
-**Requirements:** Windows 10 (build 19041+) or Windows 11
-
-### Step 1: Install WSL2
-
-Open PowerShell as Administrator:
-
-```powershell
-# Install WSL2 with Ubuntu
-wsl --install -d Ubuntu-22.04
-
-# set the user name and password
-# Then update the system
-sudo apt update
-sudo apt upgrade
-
-# Restart computer if prompted
-```
-
-### Step 2: Enable GUI Support (WSLg)
-
-Windows 11 and recent Windows 10 updates include WSLg (GUI support) automatically.
-
-Verify by opening Ubuntu and running:
-```bash
-# Test GUI (should open a window)
-sudo apt update
-sudo apt install x11-apps -y
-xclock
-```
-
-If xclock appears, GUI is working!
-
-### Step 3: Install Simulation in WSL
-
-Open Ubuntu from Start menu:
-
-```bash
-# Clone or copy your project
-cd ~
-git clone <your-repo-url> simulation
-# OR copy from Windows:
-# cp -r /mnt/c/Users/YourName/simulation ~/simulation
-
-cd simulation
-
-# Run Ubuntu installer
-./setup/install_ubuntu.sh
-
-# Activate
-source activate.sh
-
-# Run with GUI
-python standalone_simulation.py
-```
-
-### WSL2 Tips
-
-**Access Windows files:**
-```bash
-# Windows C: drive is at /mnt/c/
-cd /mnt/c/Users/YourName/Documents
-```
-
-**Run from Windows Terminal:**
-```powershell
-wsl -d Ubuntu-22.04 -e bash -c "cd ~/simulation && source activate.sh && python standalone_simulation.py"
-```
-
-**GPU Acceleration (NVIDIA):**
-
-If you have an NVIDIA GPU:
-```bash
-# Check if GPU is available
-nvidia-smi
-
-# PyBullet will use hardware rendering automatically
-```
-
-**Install Gazebo (optional):**
-
-If you want to use Gazebo simulation:
-```bash
-# Install ros-gz bridge
-sudo apt install ros-humble-ros-gz
-
-# Install Gazebo Fortress (provides 'ign' command)
-sudo apt install gz-fortress
-
-# Verify - one of these should work:
-gz sim --version      # Newer Gazebo
-ign gazebo --version  # Fortress (ROS 2 Humble)
-```
-
-**Note:** ROS 2 Humble uses Gazebo Fortress, which uses `ign gazebo` command instead of `gz sim`. The launch file auto-detects which command is available.
-
-**Gazebo GPU Issues in WSL2:**
-
-If Gazebo crashes with GPU/OpenGL errors, try:
-
-```bash
-# Option 1: Run in server mode (no GUI)
-ign gazebo -s gazebo/worlds/drone_landing.sdf
-
-# Option 2: Fix permissions and restart WSL
-sudo usermod -aG render $USER
-chmod 700 /run/user/1000
-# Then in PowerShell: wsl --shutdown
-
-# Option 3: Force software rendering
-export LIBGL_ALWAYS_SOFTWARE=1
-ign gazebo gazebo/worlds/drone_landing.sdf
-
-# Option 4: Just use PyBullet (more reliable on WSL2)
-python standalone_simulation.py
-```
-
-**Troubleshooting WSL GUI:**
-
-If GUI doesn't work:
-```bash
-# Update WSL
-wsl --update
-
-# Set WSL2 as default
-wsl --set-default-version 2
-
-# Reinstall Ubuntu
-wsl --unregister Ubuntu-22.04
-wsl --install -d Ubuntu-22.04
-```
-
-**Using VcXsrv (older Windows 10):**
-
-If WSLg isn't available:
-```powershell
-# Install VcXsrv
-choco install vcxsrv -y
-```
-
-Then in WSL:
-```bash
-# Add to ~/.bashrc
-export DISPLAY=$(grep -m 1 nameserver /etc/resolv.conf | awk '{print $2}'):0
-export LIBGL_ALWAYS_INDIRECT=1
-
-# Start VcXsrv with "Disable access control" checked
-# Then run simulation
-python standalone_simulation.py
-```
-
----
-
-## Manual Installation
-
-If the install scripts don't work, install manually:
-
-### 1. Python 3.10+
-
-```bash
-# Ubuntu/Debian
-sudo apt install python3 python3-pip python3-venv
-
-# Arch
-sudo pacman -S python python-pip python-virtualenv
-
-# macOS
-brew install python@3.11
-
-# Windows
-# Download from https://python.org
-```
-
-### 2. Create Virtual Environment
 
 ```bash
+# Create virtual environment
 python3 -m venv venv
-source venv/bin/activate  # Linux/macOS
+source venv/bin/activate
-# OR
-.\venv\Scripts\Activate.ps1  # Windows
-```
 
-### 3. Install Python Packages
+# Install packages
-
-```bash
 pip install -r requirements.txt
-```
 
-Or manually:
+# Run
-```bash
-pip install pybullet numpy pillow pyinstaller
-```
-
-### 4. Run Simulation
-
-```bash
 python standalone_simulation.py
 ```
 
 ---
 
-## Troubleshooting
-
-### PyBullet fails to install
-
-Install build tools:
-
-```bash
-# Ubuntu/Debian
-sudo apt install build-essential
-
-# Arch
-sudo pacman -S base-devel
-
-# macOS
-xcode-select --install
-
-# Windows
-# Install Visual Studio Build Tools
-```
-
-### "Cannot connect to X server"
-
-PyBullet GUI requires a display:
-
-```bash
-# Use virtual display
-sudo apt install xvfb
-xvfb-run python standalone_simulation.py
-
-# OR use X11 forwarding
-ssh -X user@host
-```
-
-### Pillow fails to install
-
-```bash
-# Ubuntu/Debian
-sudo apt install libjpeg-dev zlib1g-dev
-
-# Arch
-sudo pacman -S libjpeg-turbo zlib
-
-# macOS
-brew install libjpeg zlib
-```
-
-### Permission denied on Windows
-
-Run PowerShell as Administrator:
-```powershell
-Set-ExecutionPolicy RemoteSigned -Scope CurrentUser
-```
-
----
-
 ## Verification
 
-After installation, verify packages:
-
 ```bash
 python -c "import pybullet; print('PyBullet OK')"
-python -c "import numpy; print('NumPy OK')"
+python -c "import cv2; print('OpenCV OK')"
-python -c "from PIL import Image; print('Pillow OK')"
 python -c "from pymavlink import mavutil; print('pymavlink OK')"
 ```
-
-All should print "OK".
-
----
-
-## ArduPilot SITL Manual Setup
-
-If you want to install ArduPilot SITL manually (without the install script):
-
-### 1. Install ArduPilot
-
-```bash
-# Clone ArduPilot
-git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git ~/ardupilot
-cd ~/ardupilot
-
-# Install prerequisites (Ubuntu)
-Tools/environment_install/install-prereqs-ubuntu.sh -y
-
-# Reload profile
-. ~/.profile
-
-# Build ArduCopter SITL
-./waf configure --board sitl
-./waf copter
-```
-
-### 2. Install ArduPilot Gazebo Plugin
-
-```bash
-# Clone plugin
-git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo
-cd ~/ardupilot_gazebo
-
-# Build
-mkdir build && cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j$(nproc)
-```
-
-### 3. Set Environment Variables
-
-Add to `~/.bashrc`:
-
-```bash
-# ArduPilot
-export ARDUPILOT_HOME=$HOME/ardupilot
-export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest
-
-# ArduPilot Gazebo Plugin
-export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH
-export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH
-```
-
-### 4. Test SITL
-
-```bash
-# Test ArduCopter SITL
-cd ~/ardupilot
-sim_vehicle.py -v ArduCopter --console --map
-```
-
-### 5. Run with Gazebo
-
-```bash
-# Terminal 1: Launch Gazebo
-ros2 launch gazebo/launch/ardupilot_drone.launch.py
-
-# Terminal 2: Start SITL
-cd ~/ardupilot
-sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
-
-# Terminal 3: Run controllers
-cd ~/simulation
-source activate.sh
-python run_ardupilot.py --no-sitl --pattern circular
-```
-
-For more details, see [ArduPilot Guide](ardupilot.md).

docs/protocol.md

@@ -1,8 +1,8 @@
-# Communication Protocol (GPS-Denied)
+# Communication Protocol
 
-Message formats for GPS-denied drone operation with camera.
+Message formats for drone operation.
 
-## Drone Commands
+## Commands
 
 ```json
 {
@@ -13,25 +13,20 @@ Message formats for GPS-denied drone operation with camera.
 }
 ```
 
-| Field | Range | Description |
+| Field | Range | Effect |
-|-------|-------|-------------|
+|-------|-------|--------|
-| `thrust` | ±1.0 | Vertical thrust (positive = up) |
+| thrust | ±1.0 | Up/down |
-| `pitch` | ±0.5 | Forward/backward tilt |
+| pitch | ±0.5 | Forward/back |
-| `roll` | ±0.5 | Left/right tilt |
+| roll | ±0.5 | Left/right |
-| `yaw` | ±0.5 | Rotation |
+| yaw | ±0.5 | Rotation |
 
----
+## Telemetry
-
-## Drone Telemetry
-
-Published on `/drone/telemetry`. **No GPS position available.**
 
 ```json
 {
     "imu": {
         "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0},
-        "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
+        "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}
-        "linear_acceleration": {"x": 0.0, "y": 0.0, "z": 9.81}
     },
     "altimeter": {
         "altitude": 5.0,
@@ -47,138 +42,30 @@ Published on `/drone/telemetry`. **No GPS position available.**
     "camera": {
         "width": 320,
         "height": 240,
-        "fov": 60.0,
+        "image": "<base64 JPEG>"
-        "image": "<base64 encoded JPEG>"
+    }
-    },
-    "landed": false,
-    "timestamp": 1.234
 }
 ```
 
----
+## Sensors
 
-## Sensor Details
+| Sensor | Fields |
+|--------|--------|
+| IMU | orientation (roll, pitch, yaw), angular_velocity |
+| Altimeter | altitude, vertical_velocity |
+| Velocity | x, y, z (m/s) |
+| Landing Pad | relative_x, relative_y, distance, confidence |
+| Camera | Base64 JPEG image |
 
-### IMU
+## Decoding Camera
-Always available.
-
-| Field | Unit | Description |
-|-------|------|-------------|
-| `orientation.roll/pitch/yaw` | radians | Euler angles |
-| `angular_velocity.x/y/z` | rad/s | Rotation rates |
-| `linear_acceleration.x/y/z` | m/s² | Acceleration |
-
-### Altimeter
-Always available.
-
-| Field | Unit | Description |
-|-------|------|-------------|
-| `altitude` | meters | Height above ground |
-| `vertical_velocity` | m/s | Vertical speed |
-
-### Velocity
-Estimated from optical flow.
-
-| Field | Unit | Description |
-|-------|------|-------------|
-| `x` | m/s | Forward velocity |
-| `y` | m/s | Lateral velocity |
-| `z` | m/s | Vertical velocity |
-
-### Landing Pad Detection
-**May be null if pad not visible!**
-
-| Field | Unit | Description |
-|-------|------|-------------|
-| `relative_x` | meters | Forward/back offset (body frame) |
-| `relative_y` | meters | Left/right offset (body frame) |
-| `distance` | meters | Vertical distance to pad |
-| `confidence` | 0-1 | Detection confidence |
-
-### Camera
-Always available.
-
-| Field | Description |
-|-------|-------------|
-| `width` | Image width in pixels |
-| `height` | Image height in pixels |
-| `fov` | Horizontal field of view in degrees |
-| `image` | Base64 encoded JPEG (or null) |
-
----
-
-## Using the Camera Image
-
-The camera provides a base64-encoded JPEG image of what the drone sees looking down.
-
-### Decoding the Image (Python)
-
-```python
-import base64
-from PIL import Image
-import io
-
-def decode_camera_image(telemetry):
-    camera = telemetry.get('camera', {})
-    image_b64 = camera.get('image')
-    
-    if image_b64 is None:
-        return None
-    
-    # Decode base64 to bytes
-    image_bytes = base64.b64decode(image_b64)
-    
-    # Load as PIL Image
-    image = Image.open(io.BytesIO(image_bytes))
-    
-    return image
-```
-
-### Using with OpenCV
 
 ```python
 import base64
 import cv2
 import numpy as np
 
-def decode_camera_image_cv2(telemetry):
+image_b64 = telemetry['camera']['image']
-    camera = telemetry.get('camera', {})
-    image_b64 = camera.get('image')
-    
-    if image_b64 is None:
-        return None
-    
-    # Decode base64 to bytes
 image_bytes = base64.b64decode(image_b64)
-    
-    # Convert to numpy array
 nparr = np.frombuffer(image_bytes, np.uint8)
-    
-    # Decode JPEG
 image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
-    
-    return image
-```
-
-### Image Properties
-
-- **Resolution**: 320 x 240 pixels
-- **Format**: JPEG (quality 70)
-- **FOV**: 60 degrees
-- **Direction**: Downward-facing
-- **Update Rate**: ~5 Hz (every 5th telemetry frame)
-
----
-
-## Rover Telemetry
-
-For internal use by RoverController.
-
-```json
-{
-    "position": {"x": 1.5, "y": 0.8, "z": 0.15},
-    "velocity": {"x": 0.3, "y": 0.4, "z": 0.0},
-    "pattern": "circular",
-    "timestamp": 1.234
-}
 ```

docs/pybullet.md

@@ -1,163 +1,68 @@
 # PyBullet Simulation Guide
 
-Running the GPS-denied drone simulation with PyBullet physics engine.
+## Standalone Mode (1 Terminal)
 
-## Standalone Mode (Single Terminal - Any Platform)
+No ROS 2 required. Works on Windows, macOS, Linux:
-
-No ROS 2 required! Works on Windows, macOS, and Linux:
 
 ```bash
-source activate.sh  # Linux/macOS
+source activate.sh
-. .\activate.ps1    # Windows
+python standalone_simulation.py --pattern circular
-
-python standalone_simulation.py --pattern circular --speed 0.3
 ```
 
-### Options
+## ROS 2 Mode (2 Terminals)
-
-```bash
-python standalone_simulation.py --help
-
-Options:
-  --pattern, -p   stationary, linear, circular, square
-  --speed, -s     Rover speed in m/s (default: 0.5)
-  --amplitude, -a Movement amplitude in meters (default: 2.0)
-```
-
----
-
-## ROS 2 Mode (Two Terminals)
-
-For distributed or remote simulation with ROS 2:
 
 **Terminal 1 - Simulator:**
 ```bash
-source activate.sh
 python simulation_host.py
 ```
 
 **Terminal 2 - Controllers:**
 ```bash
-source activate.sh
+python run_bridge.py --pattern circular
-python run_bridge.py --pattern circular --speed 0.3
 ```
 
-### How It Works
+## Options
 
-1. `simulation_host.py` runs PyBullet physics and listens on UDP port 5555
-2. `run_bridge.py` starts:
-   - `ROS2SimulatorBridge` - connects ROS topics to UDP
-   - `DroneController` - your landing algorithm
-   - `RoverController` - moves the landing pad
-
-The rover position is sent to the simulator, so both drone AND rover move!
-
-### Remote Setup
-
-Run simulator on one machine, controllers on another:
-
-**Machine 1 (with display):**
 ```bash
-python simulation_host.py  # Listens on 0.0.0.0:5555
+--pattern, -p   stationary, linear, circular, square, random
+--speed, -s     Rover speed in m/s (default: 0.5)
+--amplitude, -a Movement radius (default: 2.0)
 ```
 
-**Machine 2 (headless):**
+## Remote Setup
-```bash
-python run_bridge.py --host 192.168.1.100 --pattern circular
-```
 
----
+**Machine 1:** `python simulation_host.py`
+**Machine 2:** `python run_bridge.py --host <IP>`
 
-## Configuration
+## Sensors
-
-All parameters are configurable in `config.py`:
-
-```python
-DRONE = {
-    "mass": 1.0,
-    "start_position": (0.0, 0.0, 5.0),
-    "thrust_scale": 15.0,
-    ...
-}
-
-ROVER = {
-    "start_position": (0.0, 0.0, 0.15),
-    "default_pattern": "circular",
-    "default_speed": 0.5,
-    ...
-}
-
-CONTROLLER = {
-    "Kp_z": 0.5,
-    "Kd_z": 0.3,
-    ...
-}
-```
-
----
-
-## Simulation Parameters
-
-| Parameter | Value |
-|-----------|-------|
-| Physics Rate | 240 Hz |
-| Telemetry Rate | 24 Hz |
-| Drone Mass | 1.0 kg (configurable) |
-| Rover Mass | Static (kinematic) |
-| UDP Port | 5555 (commands), 5556 (telemetry) |
-
-## GPS-Denied Sensors
 
 | Sensor | Description |
 |--------|-------------|
-| **IMU** | Orientation (roll, pitch, yaw), angular velocity |
+| IMU | Orientation, angular velocity |
-| **Altimeter** | Altitude above ground, vertical velocity |
+| Altimeter | Altitude, vertical velocity |
-| **Velocity** | Estimated horizontal velocity (x, y, z) |
+| Velocity | Estimated velocity (x, y, z) |
-| **Camera** | 320x240 downward-facing JPEG image |
+| Camera | 320x240 downward JPEG |
-| **Landing Pad** | Vision-based relative position when in camera FOV |
+| Landing Pad | Relative position |
 
-## Troubleshooting
+## Configuration
 
-### "Cannot connect to X server"
+Edit `config.py`:
 
-PyBullet GUI requires a display:
-```bash
-# Use virtual display
-xvfb-run python standalone_simulation.py
-
-# Or use X11 forwarding
-ssh -X user@host
-```
-
-### Drone flies erratically
-
-Reduce control gains in `config.py`:
 ```python
 CONTROLLER = {
-    "Kp_z": 0.3,
+    "Kp_z": 0.5,
-    "Kd_z": 0.2,
+    "Kd_z": 0.3,
-    "Kp_xy": 0.2,
+    "Kp_xy": 0.3,
-    "Kd_xy": 0.1,
+    "Kd_xy": 0.2,
 }
 ```
 
-### Camera image not appearing
+## Troubleshooting
 
-Install Pillow:
+**"Cannot connect to X server":**
 ```bash
-pip install pillow numpy
+xvfb-run python standalone_simulation.py
 ```
 
-### Rover not moving (ROS 2 mode)
+**Drone flies erratically:**
-
+Reduce gains in `config.py`
-Ensure `run_bridge.py` is used (not `ros_bridge.py` directly).
-The rover controller must be running to send position updates.
-
-### WSL2 GUI issues
-
-Set display scaling:
-```bash
-export GDK_DPI_SCALE=1.0
-export QT_SCALE_FACTOR=1.0
-python standalone_simulation.py
-```

docs/rover_controller.md

@@ -4,97 +4,58 @@ The RoverController creates a moving landing pad target.
 
 ## Usage
 
-The rover controller is automatically included when running `controllers.py`:
-
 ```bash
-# Stationary rover (default)
+# Stationary (default)
-python controllers.py
+python standalone_simulation.py --pattern stationary
 
-# Moving rover
+# Moving
-python controllers.py --pattern circular --speed 0.3
+python standalone_simulation.py --pattern circular --speed 0.3
 ```
 
-### Options
+## Options
 
-| Option | Short | Default | Description |
+| Option | Default | Description |
-|--------|-------|---------|-------------|
+|--------|---------|-------------|
-| `--pattern` | `-p` | stationary | Movement pattern |
+| `--pattern, -p` | stationary | Movement pattern |
-| `--speed` | `-s` | 0.5 | Speed in m/s |
+| `--speed, -s` | 0.5 | Speed in m/s |
-| `--amplitude` | `-a` | 2.0 | Amplitude in meters |
+| `--amplitude, -a` | 2.0 | Radius in meters |
 
-## Movement Patterns
+## Patterns
 
-### Stationary
+| Pattern | Description |
-```bash
+|---------|-------------|
-python controllers.py --pattern stationary
+| stationary | Stays at origin |
-```
+| linear | Oscillates along X-axis |
-Rover stays at origin. Best for initial testing.
+| circular | Circular path |
-
+| square | Square with sharp turns |
-### Linear
+| random | Random positions |
-```bash
-python controllers.py --pattern linear --speed 0.3 --amplitude 2.0
-```
-Oscillates along X-axis.
-
-### Circular
-```bash
-python controllers.py --pattern circular --speed 0.5 --amplitude 2.0
-```
-Follows circular path of radius `amplitude`.
-
-### Random
-```bash
-python controllers.py --pattern random --speed 0.3 --amplitude 2.0
-```
-Moves to random positions. Changes target every 3 seconds.
-
-### Square
-```bash
-python controllers.py --pattern square --speed 0.5 --amplitude 2.0
-```
-Square pattern with corners at `(±amplitude, ±amplitude)`.
 
 ## Difficulty Levels
 
-| Level | Pattern | Speed | Description |
+| Level | Pattern | Speed |
-|-------|---------|-------|-------------|
+|-------|---------|-------|
-| Beginner | stationary | 0.0 | Static target |
+| Beginner | stationary | 0.0 |
-| Easy | linear | 0.2 | Predictable 1D |
+| Easy | linear | 0.2 |
-| Medium | circular | 0.3 | Smooth 2D |
+| Medium | circular | 0.3 |
-| Hard | random | 0.3 | Unpredictable |
+| Hard | random | 0.3 |
-| Expert | square | 0.5 | Sharp turns |
+| Expert | square | 0.5 |
 
 ## Progressive Testing
 
-Start easy and increase difficulty:
-
 ```bash
-# Step 1: Static target
+# 1. Static target
-python controllers.py --pattern stationary
+python standalone_simulation.py --pattern stationary
 
-# Step 2: Slow linear motion
+# 2. Slow circular
-python controllers.py --pattern linear --speed 0.2
+python standalone_simulation.py --pattern circular --speed 0.2
 
-# Step 3: Slow circular motion
+# 3. Faster circular
-python controllers.py --pattern circular --speed 0.2
+python standalone_simulation.py --pattern circular --speed 0.4
 
-# Step 4: Faster circular
+# 4. Random
-python controllers.py --pattern circular --speed 0.4
+python standalone_simulation.py --pattern random --speed 0.3
-
-# Step 5: Random
-python controllers.py --pattern random --speed 0.3
 ```
 
-## Published Topics
+## Note
 
-| Topic | Type | Description |
+The drone cannot access rover position directly (GPS-denied). It must detect the landing pad visually via the camera.
-|-------|------|-------------|
-| `/rover/cmd_vel` | `Twist` | Velocity commands |
-| `/rover/position` | `Point` | Current position |
-| `/rover/telemetry` | `String` | Full state (JSON) |
-
-## GPS-Denied Note
-
-In GPS-denied mode, the drone cannot directly access rover position. Instead, it must detect the landing pad visually via `landing_pad` sensor data.
-
-The `/rover/telemetry` topic is used internally by the RoverController but the DroneController should primarily rely on vision-based `landing_pad` detection in the drone telemetry.

drone_controller.py

@@ -1,16 +1,24 @@
 #!/usr/bin/env python3
 """
 DroneController - Template for GPS-denied landing logic.
+
+Supports multiple telemetry sources:
+  1. Custom JSON telemetry (/drone/telemetry) - PyBullet/Gazebo bridge
+  2. ArduPilot ROS 2 DDS topics (/ap/*) - Official ArduPilot integration
+
 Implement your algorithm in calculate_landing_maneuver().
 Uses sensors: IMU, altimeter, camera, and landing pad detection.
 """
 
 import json
+import math
 from typing import Dict, Any, Optional
 
 import rclpy
 from rclpy.node import Node
-from geometry_msgs.msg import Twist
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+from geometry_msgs.msg import Twist, PoseStamped, TwistStamped
+from sensor_msgs.msg import Imu, BatteryState, NavSatFix
 from std_msgs.msg import String
 
 # Load configuration
@@ -24,10 +32,38 @@ except ImportError:
     LANDING = {"height_threshold": 0.1, "success_velocity": 0.1}
 
 
-class DroneController(Node):
+def quaternion_to_euler(x: float, y: float, z: float, w: float) -> tuple:
-    """Drone controller for GPS-denied landing."""
+    """Convert quaternion to Euler angles (roll, pitch, yaw)."""
+    # Roll (x-axis rotation)
+    sinr_cosp = 2.0 * (w * x + y * z)
+    cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
+    roll = math.atan2(sinr_cosp, cosr_cosp)
     
-    def __init__(self):
+    # Pitch (y-axis rotation)
+    sinp = 2.0 * (w * y - z * x)
+    if abs(sinp) >= 1:
+        pitch = math.copysign(math.pi / 2, sinp)
+    else:
+        pitch = math.asin(sinp)
+    
+    # Yaw (z-axis rotation)
+    siny_cosp = 2.0 * (w * z + x * y)
+    cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
+    yaw = math.atan2(siny_cosp, cosy_cosp)
+    
+    return roll, pitch, yaw
+
+
+class DroneController(Node):
+    """
+    Drone controller for GPS-denied landing.
+    
+    Supports multiple telemetry sources:
+      - Legacy: /drone/telemetry (JSON String)
+      - ArduPilot DDS: /ap/pose/filtered, /ap/imu/filtered, etc.
+    """
+
+    def __init__(self, use_ardupilot_topics: bool = True):
         super().__init__('drone_controller')
         
         # Load from config
@@ -42,37 +78,189 @@ class DroneController(Node):
         self._landing_height = LANDING.get("height_threshold", 0.1)
         self._landing_velocity = LANDING.get("success_velocity", 0.1)
         
+        # Mode selection
+        self._use_ardupilot = use_ardupilot_topics
+        
         self.get_logger().info('=' * 50)
         self.get_logger().info('Drone Controller Starting (GPS-Denied)...')
         if CONFIG_LOADED:
             self.get_logger().info('  Configuration loaded from config.py')
+        self.get_logger().info(f'  Mode: {"ArduPilot DDS" if use_ardupilot_topics else "Legacy JSON"}')
         self.get_logger().info('=' * 50)
         
+        # State variables
         self._latest_telemetry: Optional[Dict[str, Any]] = None
         self._rover_telemetry: Optional[Dict[str, Any]] = None
         self._telemetry_received = False
         self._landing_complete = False
         
-        self._telemetry_sub = self.create_subscription(
+        # ArduPilot state (built from DDS topics)
-            String, '/drone/telemetry', self._telemetry_callback, 10
+        self._ap_pose: Optional[PoseStamped] = None
-        )
+        self._ap_twist: Optional[TwistStamped] = None
-        self.get_logger().info('  Subscribed to: /drone/telemetry')
+        self._ap_imu: Optional[Imu] = None
+        self._ap_battery: Optional[BatteryState] = None
         
+        # QoS for sensor topics
+        sensor_qos = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1
+        )
+        
+        if use_ardupilot_topics:
+            self._setup_ardupilot_subscriptions(sensor_qos)
+        else:
+            self._setup_legacy_subscriptions()
+        
+        # Always subscribe to rover telemetry (for moving landing pad)
         self._rover_telemetry_sub = self.create_subscription(
             String, '/rover/telemetry', self._rover_telemetry_callback, 10
         )
         self.get_logger().info('  Subscribed to: /rover/telemetry')
         
+        # Command publisher
         self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
         self.get_logger().info('  Publishing to: /cmd_vel')
         
+        # Control loop
         control_period = 1.0 / self._control_rate
         self._control_timer = self.create_timer(control_period, self._control_loop)
         self.get_logger().info(f'  Control loop: {self._control_rate} Hz')
         
         self.get_logger().info('Drone Controller Ready!')
-        self.get_logger().info('Sensors: IMU, Altimeter, Camera, Landing Pad Detection')
+        self.get_logger().info('Sensors: IMU, Altimeter, Velocity, Landing Pad Detection')
     
+    def _setup_ardupilot_subscriptions(self, qos: QoSProfile):
+        """Set up subscriptions to official ArduPilot ROS 2 topics."""
+        
+        # Pose (position + orientation)
+        self._ap_pose_sub = self.create_subscription(
+            PoseStamped, '/ap/pose/filtered', self._ap_pose_callback, qos
+        )
+        self.get_logger().info('  Subscribed to: /ap/pose/filtered')
+        
+        # Twist (velocity)
+        self._ap_twist_sub = self.create_subscription(
+            TwistStamped, '/ap/twist/filtered', self._ap_twist_callback, qos
+        )
+        self.get_logger().info('  Subscribed to: /ap/twist/filtered')
+        
+        # IMU
+        self._ap_imu_sub = self.create_subscription(
+            Imu, '/ap/imu/filtered', self._ap_imu_callback, qos
+        )
+        self.get_logger().info('  Subscribed to: /ap/imu/filtered')
+        
+        # Battery
+        self._ap_battery_sub = self.create_subscription(
+            BatteryState, '/ap/battery', self._ap_battery_callback, qos
+        )
+        self.get_logger().info('  Subscribed to: /ap/battery')
+    
+    def _setup_legacy_subscriptions(self):
+        """Set up subscriptions to legacy JSON telemetry."""
+        self._telemetry_sub = self.create_subscription(
+            String, '/drone/telemetry', self._telemetry_callback, 10
+        )
+        self.get_logger().info('  Subscribed to: /drone/telemetry')
+    
+    # ArduPilot topic callbacks
+    def _ap_pose_callback(self, msg: PoseStamped):
+        self._ap_pose = msg
+        if not self._telemetry_received:
+            self._telemetry_received = True
+            self._warmup_count = 0
+            self.get_logger().info('First ArduPilot pose received!')
+        self._build_telemetry_from_ardupilot()
+    
+    def _ap_twist_callback(self, msg: TwistStamped):
+        self._ap_twist = msg
+        self._build_telemetry_from_ardupilot()
+    
+    def _ap_imu_callback(self, msg: Imu):
+        self._ap_imu = msg
+        self._build_telemetry_from_ardupilot()
+    
+    def _ap_battery_callback(self, msg: BatteryState):
+        self._ap_battery = msg
+        self._build_telemetry_from_ardupilot()
+    
+    def _build_telemetry_from_ardupilot(self):
+        """Build telemetry dict from ArduPilot topics."""
+        telemetry = {}
+        
+        # Position and orientation from pose
+        if self._ap_pose:
+            pos = self._ap_pose.pose.position
+            ori = self._ap_pose.pose.orientation
+            roll, pitch, yaw = quaternion_to_euler(ori.x, ori.y, ori.z, ori.w)
+            
+            telemetry['position'] = {
+                'x': pos.x,
+                'y': pos.y,
+                'z': pos.z
+            }
+            telemetry['altimeter'] = {
+                'altitude': pos.z,
+                'vertical_velocity': self._ap_twist.twist.linear.z if self._ap_twist else 0.0
+            }
+            telemetry['imu'] = {
+                'orientation': {'roll': roll, 'pitch': pitch, 'yaw': yaw},
+                'angular_velocity': {'x': 0, 'y': 0, 'z': 0}
+            }
+        
+        # Velocity from twist
+        if self._ap_twist:
+            twist = self._ap_twist.twist
+            telemetry['velocity'] = {
+                'x': twist.linear.x,
+                'y': twist.linear.y,
+                'z': twist.linear.z
+            }
+            if 'altimeter' in telemetry:
+                telemetry['altimeter']['vertical_velocity'] = twist.linear.z
+        
+        # Angular velocity from IMU
+        if self._ap_imu:
+            if 'imu' not in telemetry:
+                telemetry['imu'] = {}
+            telemetry['imu']['angular_velocity'] = {
+                'x': self._ap_imu.angular_velocity.x,
+                'y': self._ap_imu.angular_velocity.y,
+                'z': self._ap_imu.angular_velocity.z
+            }
+        
+        # Battery
+        if self._ap_battery:
+            telemetry['battery'] = {
+                'voltage': self._ap_battery.voltage,
+                'remaining': self._ap_battery.percentage * 100
+            }
+        
+        # Landing pad detection (placeholder - would need vision processing)
+        # For now, calculate from rover telemetry if available
+        if self._rover_telemetry and self._ap_pose:
+            rover_pos = self._rover_telemetry.get('position', {})
+            rx = rover_pos.get('x', 0)
+            ry = rover_pos.get('y', 0)
+            dx = self._ap_pose.pose.position.x
+            dy = self._ap_pose.pose.position.y
+            dz = self._ap_pose.pose.position.z
+            
+            rel_x = rx - dx
+            rel_y = ry - dy
+            distance = math.sqrt(rel_x**2 + rel_y**2 + dz**2)
+            
+            telemetry['landing_pad'] = {
+                'relative_x': rel_x,
+                'relative_y': rel_y,
+                'distance': distance,
+                'confidence': 1.0 if distance < 10.0 else 0.0
+            }
+        
+        self._latest_telemetry = telemetry
+    
+    # Legacy callbacks
     def _telemetry_callback(self, msg: String) -> None:
         try:
             self._latest_telemetry = json.loads(msg.data)
@@ -211,11 +399,19 @@ class DroneController(Node):
 
 
 def main(args=None):
-    rclpy.init(args=args)
+    import sys
+    
+    # Check for --ardupilot flag
+    use_ardupilot = '--ardupilot' in sys.argv or '-a' in sys.argv
+    
+    # Remove our custom args before passing to rclpy
+    filtered_args = [a for a in (args or sys.argv) if a not in ['--ardupilot', '-a']]
+    
+    rclpy.init(args=filtered_args)
     controller = None
     
     try:
-        controller = DroneController()
+        controller = DroneController(use_ardupilot_topics=use_ardupilot)
         rclpy.spin(controller)
     except KeyboardInterrupt:
         print('\nShutting down...')

gazebo/launch/ardupilot_drone.launch.py

@@ -1,40 +1,52 @@
 #!/usr/bin/env python3
 """
-ArduPilot SITL + Gazebo Launch File
+ArduPilot ROS 2 Launch Helper
-Works with Gazebo Harmonic/Garden and ArduPilot SITL.
 
-This launch file:
+This file provides a helper for launching the official ArduPilot ROS 2 simulation.
-1. Starts Gazebo with ArduPilot-compatible drone model
+For the full ArduPilot + Gazebo experience, use the official ardupilot_gz packages.
-2. Sets up ROS-Gazebo bridge for telemetry
-3. Optionally starts ArduPilot SITL
 
-NOTE: ArduPilot SITL integration requires the ardupilot_gazebo plugin.
+RECOMMENDED: Use the official launch files:
-Install from: https://github.com/ArduPilot/ardupilot_gazebo
+    ros2 launch ardupilot_gz_bringup iris_runway.launch.py
+
+This local launch file is for custom Gazebo worlds or fallback testing.
 """
 
 import os
 import shutil
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction, LogInfo
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
-    """Generate the launch description."""
+    """
+    Generate launch description for ArduPilot simulation.
+    
+    This launch file:
+    1. Checks for official ardupilot_gz packages
+    2. Falls back to local Gazebo world if packages not found
+    3. Starts ROS-Gazebo bridge for additional topics
+    """
     
     # Get paths
     script_dir = os.path.dirname(os.path.abspath(__file__))
     gazebo_dir = os.path.dirname(script_dir)
-    world_file = os.path.join(gazebo_dir, 'worlds', 'ardupilot_drone.sdf')
+    world_file = os.path.join(gazebo_dir, 'worlds', 'drone_landing.sdf')
     
-    # Check for ArduPilot Gazebo plugin
+    # Check if official ArduPilot packages are available
-    plugin_paths = [
+    try:
-        os.path.expanduser("~/ardupilot_gazebo/build"),
+        import subprocess
-        "/opt/ardupilot_gazebo/lib",
+        result = subprocess.run(
-        os.environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", ""),
+            ['ros2', 'pkg', 'prefix', 'ardupilot_gz_bringup'],
-    ]
+            capture_output=True, text=True
-    ardupilot_plugin_found = any(os.path.exists(p) for p in plugin_paths if p)
+        )
+        official_pkg_available = result.returncode == 0
+    except Exception:
+        official_pkg_available = False
+    
+    # Use simulation time
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
     
     # Determine Gazebo command
     if shutil.which('gz'):
@@ -44,37 +56,35 @@ def generate_launch_description():
     else:
         gz_cmd = ['gz', 'sim', '-r', world_file]
     
-    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
-    start_sitl = LaunchConfiguration('start_sitl', default='false')
-    
-    # Set plugin path for ArduPilot
-    env = os.environ.copy()
-    plugin_search_paths = ':'.join([p for p in plugin_paths if p and os.path.exists(p)])
-    if plugin_search_paths:
-        env['GZ_SIM_SYSTEM_PLUGIN_PATH'] = plugin_search_paths + ':' + env.get('GZ_SIM_SYSTEM_PLUGIN_PATH', '')
-    
     actions = [
         DeclareLaunchArgument(
             'use_sim_time',
             default_value='true',
             description='Use simulation clock'
         ),
+    ]
     
-        DeclareLaunchArgument(
+    if official_pkg_available:
-            'start_sitl',
+        # Use official ArduPilot packages
-            default_value='false',
+        actions.append(
-            description='Start ArduPilot SITL automatically'
+            LogInfo(msg=[
-        ),
+                '\n',
+                '=' * 60,
+                '\n  RECOMMENDED: Use official ArduPilot launch files:\n',
+                '  ros2 launch ardupilot_gz_bringup iris_runway.launch.py\n',
+                '=' * 60,
+                '\n',
+            ])
+        )
     
-        # Start Gazebo with ArduPilot world
+    # Start Gazebo with local world (for custom scenarios)
+    actions.extend([
         ExecuteProcess(
             cmd=gz_cmd,
             output='screen',
-            additional_env=env
         ),
         
-        # ROS-Gazebo Bridge for telemetry and commands
+        # ROS-Gazebo Bridge
-        # Delayed start to wait for Gazebo
         TimerAction(
             period=2.0,
             actions=[
@@ -83,14 +93,13 @@ def generate_launch_description():
                     executable='parameter_bridge',
                     name='gz_bridge',
                     arguments=[
-                        # Rover velocity commands (ROS to Gazebo)
+                        # Rover velocity commands
                         '/rover/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
-                        # Odometry (from Gazebo to ROS) - fallback if MAVLink not used
+                        # Drone velocity commands
+                        '/drone/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
+                        # Odometry
                         '/model/drone/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
-                        # Camera (from Gazebo to ROS)
+                        '/model/rover/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
-                        '/drone/camera@sensor_msgs/msg/Image[gz.msgs.Image',
-                        # IMU (from Gazebo to ROS)
-                        '/imu@sensor_msgs/msg/Imu[gz.msgs.IMU',
                         # Clock
                         '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
                     ],
@@ -99,36 +108,31 @@ def generate_launch_description():
                 ),
             ]
         ),
-    ]
+    ])
     
     return LaunchDescription(actions)
 
 
 if __name__ == '__main__':
     print("=" * 60)
-    print("  ArduPilot SITL + Gazebo Launch File")
+    print("  ArduPilot ROS 2 Launch Helper")
     print("=" * 60)
     print()
-    print("This is a ROS 2 launch file for ArduPilot integration.")
+    print("RECOMMENDED: Use official ArduPilot ROS 2 packages:")
     print()
-    print("Prerequisites:")
+    print("  # Install ArduPilot ROS 2")
-    print("  1. Gazebo (gz sim or ign gazebo)")
+    print("  ./setup/install_ardupilot.sh")
-    print("  2. ros_gz_bridge package")
-    print("  3. ArduPilot Gazebo plugin (optional, for SITL)")
     print()
-    print("Usage:")
+    print("  # Source workspace")
+    print("  source ~/ardu_ws/install/setup.bash")
+    print()
+    print("  # Launch simulation (SITL + Gazebo + RViz)")
+    print("  ros2 launch ardupilot_gz_bringup iris_runway.launch.py")
+    print()
+    print("  # Connect MAVProxy")
+    print("  mavproxy.py --console --map --master=:14550")
+    print()
+    print("-" * 60)
+    print("Alternative: Use this local launch for custom worlds:")
     print("  ros2 launch gazebo/launch/ardupilot_drone.launch.py")
     print()
-    print("Then in another terminal:")
-    print("  # Start SITL")
-    print("  sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON")
-    print()
-    print("  # Or use the integrated runner")
-    print("  python run_ardupilot.py --no-sitl")
-    print()
-    print("Manual Gazebo Start:")
-    
-    if shutil.which('gz'):
-        print("  gz sim -r gazebo/worlds/ardupilot_drone.sdf")
-    else:
-        print("  ign gazebo gazebo/worlds/ardupilot_drone.sdf")

gazebo/worlds/ardupilot_drone.sdf

@@ -1,504 +0,0 @@
-<?xml version="1.0" ?>
-<!--
-  ArduPilot SITL + Gazebo Drone Landing World
-  
-  This world integrates with ArduPilot SITL via the ardupilot_gazebo plugin.
-  The drone receives motor commands from ArduPilot and sends sensor data back.
-  
-  Prerequisites:
-    1. ArduPilot Gazebo Plugin: https://github.com/ArduPilot/ardupilot_gazebo
-    2. ArduPilot SITL with JSON backend
-  
-  Usage:
-    1. Start Gazebo: gz sim -r gazebo/worlds/ardupilot_drone.sdf
-    2. Start SITL: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON
-    3. Run bridge: python run_ardupilot.py --no-sitl
--->
-<sdf version="1.9">
-  <world name="ardupilot_landing_world">
-    
-    <!-- Physics - match ArduPilot defaults -->
-    <physics name="1ms" type="ode">
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1.0</real_time_factor>
-      <real_time_update_rate>1000</real_time_update_rate>
-    </physics>
-    
-    <!-- Gazebo Plugins -->
-    <plugin filename="gz-sim-physics-system"
-            name="gz::sim::systems::Physics"/>
-    <plugin filename="gz-sim-user-commands-system"
-            name="gz::sim::systems::UserCommands"/>
-    <plugin filename="gz-sim-scene-broadcaster-system"
-            name="gz::sim::systems::SceneBroadcaster"/>
-    <plugin filename="gz-sim-sensors-system"
-            name="gz::sim::systems::Sensors">
-      <render_engine>ogre2</render_engine>
-    </plugin>
-    <plugin filename="gz-sim-imu-system"
-            name="gz::sim::systems::Imu"/>
-    <plugin filename="gz-sim-air-pressure-system"
-            name="gz::sim::systems::AirPressure"/>
-    <plugin filename="gz-sim-altimeter-system"
-            name="gz::sim::systems::Altimeter"/>
-    <plugin filename="gz-sim-navsat-system"
-            name="gz::sim::systems::NavSat"/>
-    
-    <!-- Lighting -->
-    <light type="directional" name="sun">
-      <cast_shadows>true</cast_shadows>
-      <pose>0 0 10 0 0 0</pose>
-      <diffuse>0.8 0.8 0.8 1</diffuse>
-      <specular>0.2 0.2 0.2 1</specular>
-      <direction>-0.5 0.1 -0.9</direction>
-    </light>
-    
-    <light type="directional" name="fill_light">
-      <cast_shadows>false</cast_shadows>
-      <pose>0 0 10 0 0 0</pose>
-      <diffuse>0.3 0.3 0.3 1</diffuse>
-      <specular>0.0 0.0 0.0 1</specular>
-      <direction>0.5 0.3 -0.5</direction>
-    </light>
-    
-    <!-- Ground Plane -->
-    <model name="ground_plane">
-      <static>true</static>
-      <link name="link">
-        <collision name="collision">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1</mu>
-                <mu2>1</mu2>
-              </ode>
-            </friction>
-          </surface>
-        </collision>
-        <visual name="visual">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <material>
-            <ambient>0.5 0.5 0.5 1</ambient>
-            <diffuse>0.7 0.7 0.7 1</diffuse>
-          </material>
-        </visual>
-      </link>
-    </model>
-    
-    <!-- Landing Pad (Rover) - Velocity controlled for moving target -->
-    <model name="landing_pad">
-      <pose>0 0 0.15 0 0 0</pose>
-      
-      <link name="base_link">
-        <inertial>
-          <mass>10.0</mass>
-          <inertia>
-            <ixx>1.0</ixx><ixy>0</ixy><ixz>0</ixz>
-            <iyy>1.0</iyy><iyz>0</iyz>
-            <izz>1.0</izz>
-          </inertia>
-        </inertial>
-        
-        <visual name="visual">
-          <geometry>
-            <box><size>1.0 1.0 0.3</size></box>
-          </geometry>
-          <material>
-            <ambient>0.1 0.5 0.1 1</ambient>
-            <diffuse>0.2 0.7 0.2 1</diffuse>
-          </material>
-        </visual>
-        <collision name="collision">
-          <geometry>
-            <box><size>1.0 1.0 0.3</size></box>
-          </geometry>
-        </collision>
-        
-        <!-- H marker -->
-        <visual name="h_center">
-          <pose>0 0 0.151 0 0 0</pose>
-          <geometry><box><size>0.6 0.1 0.001</size></box></geometry>
-          <material>
-            <ambient>1 1 1 1</ambient>
-            <diffuse>1 1 1 1</diffuse>
-          </material>
-        </visual>
-        <visual name="h_left">
-          <pose>-0.25 0 0.151 0 0 0</pose>
-          <geometry><box><size>0.1 0.5 0.001</size></box></geometry>
-          <material>
-            <ambient>1 1 1 1</ambient>
-            <diffuse>1 1 1 1</diffuse>
-          </material>
-        </visual>
-        <visual name="h_right">
-          <pose>0.25 0 0.151 0 0 0</pose>
-          <geometry><box><size>0.1 0.5 0.001</size></box></geometry>
-          <material>
-            <ambient>1 1 1 1</ambient>
-            <diffuse>1 1 1 1</diffuse>
-          </material>
-        </visual>
-      </link>
-      
-      <!-- Velocity control for rover -->
-      <plugin filename="gz-sim-velocity-control-system"
-              name="gz::sim::systems::VelocityControl">
-        <topic>/rover/cmd_vel</topic>
-        <initial_linear>0 0 0</initial_linear>
-      </plugin>
-    </model>
-    
-    <!-- 
-      ArduPilot Iris Quadcopter
-      
-      This model uses the ArduPilot Gazebo plugin for flight control.
-      Motor commands come from ArduPilot SITL, and sensor data is sent back.
-    -->
-    <model name="iris_with_ardupilot">
-      <pose>0 0 0.194923 0 0 0</pose>
-      
-      <link name="base_link">
-        <inertial>
-          <pose>0 0 0 0 0 0</pose>
-          <mass>1.5</mass>
-          <inertia>
-            <ixx>0.029125</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.029125</iyy>
-            <iyz>0</iyz>
-            <izz>0.055225</izz>
-          </inertia>
-        </inertial>
-        
-        <collision name="base_link_collision">
-          <geometry>
-            <box><size>0.47 0.47 0.11</size></box>
-          </geometry>
-        </collision>
-        
-        <!-- Main body -->
-        <visual name="base_link_visual">
-          <geometry>
-            <box><size>0.3 0.3 0.1</size></box>
-          </geometry>
-          <material>
-            <ambient>0.8 0.1 0.1 1</ambient>
-            <diffuse>0.9 0.2 0.2 1</diffuse>
-          </material>
-        </visual>
-        
-        <!-- Arms -->
-        <visual name="arm_fl">
-          <pose>0.12 0.12 0 0 0 0.785</pose>
-          <geometry><box><size>0.2 0.02 0.02</size></box></geometry>
-          <material><diffuse>0.3 0.3 0.3 1</diffuse></material>
-        </visual>
-        <visual name="arm_fr">
-          <pose>0.12 -0.12 0 0 0 -0.785</pose>
-          <geometry><box><size>0.2 0.02 0.02</size></box></geometry>
-          <material><diffuse>0.3 0.3 0.3 1</diffuse></material>
-        </visual>
-        <visual name="arm_bl">
-          <pose>-0.12 0.12 0 0 0 -0.785</pose>
-          <geometry><box><size>0.2 0.02 0.02</size></box></geometry>
-          <material><diffuse>0.3 0.3 0.3 1</diffuse></material>
-        </visual>
-        <visual name="arm_br">
-          <pose>-0.12 -0.12 0 0 0 0.785</pose>
-          <geometry><box><size>0.2 0.02 0.02</size></box></geometry>
-          <material><diffuse>0.3 0.3 0.3 1</diffuse></material>
-        </visual>
-        
-        <!-- GPS antenna indicator -->
-        <visual name="gps_antenna">
-          <pose>0 0 0.08 0 0 0</pose>
-          <geometry><cylinder><radius>0.02</radius><length>0.04</length></cylinder></geometry>
-          <material><diffuse>0.2 0.2 0.8 1</diffuse></material>
-        </visual>
-        
-        <!-- IMU Sensor -->
-        <sensor name="imu_sensor" type="imu">
-          <always_on>true</always_on>
-          <update_rate>400</update_rate>
-          <topic>imu</topic>
-          <imu>
-            <angular_velocity>
-              <x><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></x>
-              <y><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></y>
-              <z><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></z>
-            </angular_velocity>
-            <linear_acceleration>
-              <x><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></x>
-              <y><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></y>
-              <z><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></z>
-            </linear_acceleration>
-          </imu>
-        </sensor>
-        
-        <!-- Air Pressure Sensor (Barometer) -->
-        <sensor name="air_pressure" type="air_pressure">
-          <always_on>true</always_on>
-          <update_rate>50</update_rate>
-          <topic>air_pressure</topic>
-          <air_pressure>
-            <reference_altitude>0</reference_altitude>
-            <noise type="gaussian">
-              <mean>0</mean>
-              <stddev>0.01</stddev>
-            </noise>
-          </air_pressure>
-        </sensor>
-        
-        <!-- Downward Camera for Landing Pad Detection -->
-        <sensor name="camera" type="camera">
-          <pose>0 0 -0.05 0 1.5708 0</pose>
-          <always_on>true</always_on>
-          <update_rate>30</update_rate>
-          <topic>/drone/camera</topic>
-          <camera>
-            <horizontal_fov>1.047</horizontal_fov>
-            <image>
-              <width>320</width>
-              <height>240</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.1</near>
-              <far>100</far>
-            </clip>
-          </camera>
-        </sensor>
-      </link>
-      
-      <!-- Rotor 0: Front Right (CCW) -->
-      <link name="rotor_0">
-        <pose>0.13 -0.22 0.023 0 0 0</pose>
-        <inertial>
-          <mass>0.025</mass>
-          <inertia>
-            <ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
-            <iyy>1.66704e-04</iyy><iyz>0</iyz>
-            <izz>1.66704e-04</izz>
-          </inertia>
-        </inertial>
-        <visual name="visual">
-          <geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
-          <material><diffuse>0.1 0.1 0.1 1</diffuse></material>
-        </visual>
-        <collision name="collision">
-          <geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
-        </collision>
-      </link>
-      
-      <joint name="rotor_0_joint" type="revolute">
-        <parent>base_link</parent>
-        <child>rotor_0</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit><lower>-1e16</lower><upper>1e16</upper></limit>
-        </axis>
-      </joint>
-      
-      <!-- Rotor 1: Back Left (CCW) -->
-      <link name="rotor_1">
-        <pose>-0.13 0.2 0.023 0 0 0</pose>
-        <inertial>
-          <mass>0.025</mass>
-          <inertia>
-            <ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
-            <iyy>1.66704e-04</iyy><iyz>0</iyz>
-            <izz>1.66704e-04</izz>
-          </inertia>
-        </inertial>
-        <visual name="visual">
-          <geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
-          <material><diffuse>0.1 0.1 0.1 1</diffuse></material>
-        </visual>
-        <collision name="collision">
-          <geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
-        </collision>
-      </link>
-      
-      <joint name="rotor_1_joint" type="revolute">
-        <parent>base_link</parent>
-        <child>rotor_1</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit><lower>-1e16</lower><upper>1e16</upper></limit>
-        </axis>
-      </joint>
-      
-      <!-- Rotor 2: Front Left (CW) -->
-      <link name="rotor_2">
-        <pose>0.13 0.22 0.023 0 0 0</pose>
-        <inertial>
-          <mass>0.025</mass>
-          <inertia>
-            <ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
-            <iyy>1.66704e-04</iyy><iyz>0</iyz>
-            <izz>1.66704e-04</izz>
-          </inertia>
-        </inertial>
-        <visual name="visual">
-          <geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
-          <material><diffuse>0.1 0.1 0.1 1</diffuse></material>
-        </visual>
-        <collision name="collision">
-          <geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
-        </collision>
-      </link>
-      
-      <joint name="rotor_2_joint" type="revolute">
-        <parent>base_link</parent>
-        <child>rotor_2</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit><lower>-1e16</lower><upper>1e16</upper></limit>
-        </axis>
-      </joint>
-      
-      <!-- Rotor 3: Back Right (CW) -->
-      <link name="rotor_3">
-        <pose>-0.13 -0.2 0.023 0 0 0</pose>
-        <inertial>
-          <mass>0.025</mass>
-          <inertia>
-            <ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
-            <iyy>1.66704e-04</iyy><iyz>0</iyz>
-            <izz>1.66704e-04</izz>
-          </inertia>
-        </inertial>
-        <visual name="visual">
-          <geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
-          <material><diffuse>0.1 0.1 0.1 1</diffuse></material>
-        </visual>
-        <collision name="collision">
-          <geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
-        </collision>
-      </link>
-      
-      <joint name="rotor_3_joint" type="revolute">
-        <parent>base_link</parent>
-        <child>rotor_3</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit><lower>-1e16</lower><upper>1e16</upper></limit>
-        </axis>
-      </joint>
-      
-      <!--
-        ArduPilot Plugin Configuration
-        
-        This plugin connects to ArduPilot SITL via the JSON interface.
-        It receives motor commands and sends back sensor data.
-        
-        fdm_addr: Address of ArduPilot SITL (default 127.0.0.1)
-        fdm_port_in: Port to receive motor commands (default 9002)
-        listen_addr: Our address for sending sensor data (127.0.0.1)
-        listen_port_out: SITL port for receiving sensor data (9003)
-        
-        If ardupilot_gazebo plugin is not available, this will be ignored.
-      -->
-      <plugin filename="ArduPilotPlugin"
-              name="gz::sim::systems::ArduPilotPlugin">
-        <!-- Connection settings -->
-        <fdm_addr>127.0.0.1</fdm_addr>
-        <fdm_port_in>9002</fdm_port_in>
-        <listen_addr>127.0.0.1</listen_addr>
-        <listen_port_out>9003</listen_port_out>
-        
-        <!-- Model reference link -->
-        <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
-        <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
-        
-        <!-- IMU configuration -->
-        <imuName>imu_sensor</imuName>
-        
-        <!-- Motor configuration (CCW positive, CW negative) -->
-        <!-- Rotor 0: FR CCW -->
-        <control channel="0">
-          <jointName>rotor_0_joint</jointName>
-          <type>VELOCITY</type>
-          <offset>0</offset>
-          <multiplier>838</multiplier>
-          <p_gain>0.2</p_gain>
-          <i_gain>0</i_gain>
-          <d_gain>0</d_gain>
-          <i_max>0</i_max>
-          <i_min>0</i_min>
-          <cmd_max>2.5</cmd_max>
-          <cmd_min>-2.5</cmd_min>
-        </control>
-        
-        <!-- Rotor 1: BL CCW -->
-        <control channel="1">
-          <jointName>rotor_1_joint</jointName>
-          <type>VELOCITY</type>
-          <offset>0</offset>
-          <multiplier>838</multiplier>
-          <p_gain>0.2</p_gain>
-          <i_gain>0</i_gain>
-          <d_gain>0</d_gain>
-          <i_max>0</i_max>
-          <i_min>0</i_min>
-          <cmd_max>2.5</cmd_max>
-          <cmd_min>-2.5</cmd_min>
-        </control>
-        
-        <!-- Rotor 2: FL CW -->
-        <control channel="2">
-          <jointName>rotor_2_joint</jointName>
-          <type>VELOCITY</type>
-          <offset>0</offset>
-          <multiplier>-838</multiplier>
-          <p_gain>0.2</p_gain>
-          <i_gain>0</i_gain>
-          <d_gain>0</d_gain>
-          <i_max>0</i_max>
-          <i_min>0</i_min>
-          <cmd_max>2.5</cmd_max>
-          <cmd_min>-2.5</cmd_min>
-        </control>
-        
-        <!-- Rotor 3: BR CW -->
-        <control channel="3">
-          <jointName>rotor_3_joint</jointName>
-          <type>VELOCITY</type>
-          <offset>0</offset>
-          <multiplier>-838</multiplier>
-          <p_gain>0.2</p_gain>
-          <i_gain>0</i_gain>
-          <d_gain>0</d_gain>
-          <i_max>0</i_max>
-          <i_min>0</i_min>
-          <cmd_max>2.5</cmd_max>
-          <cmd_min>-2.5</cmd_min>
-        </control>
-      </plugin>
-      
-      <!-- Odometry publisher for compatibility -->
-      <plugin filename="gz-sim-odometry-publisher-system"
-              name="gz::sim::systems::OdometryPublisher">
-        <odom_frame>odom</odom_frame>
-        <robot_base_frame>base_link</robot_base_frame>
-        <odom_topic>/model/drone/odometry</odom_topic>
-        <odom_publish_frequency>50</odom_publish_frequency>
-      </plugin>
-      
-    </model>
-    
-  </world>
-</sdf>

mavlink_bridge.py

@@ -1,579 +1,423 @@
 #!/usr/bin/env python3
 """
-MAVLink Bridge for ArduPilot SITL Integration.
+MAVLink Command Interface for ArduPilot.
-Connects Gazebo simulation to ArduPilot SITL via MAVProxy.
 
-This bridge:
+This module provides a Python interface for sending MAVLink commands to ArduPilot
-- Connects to ArduPilot SITL via MAVLink (UDP)
+when using the official ROS 2 DDS integration. Telemetry is received via native
-- Receives telemetry from ArduPilot (position, attitude, battery, etc.)
+ROS 2 topics (/ap/*), but commands are still sent via MAVLink.
-- Sends commands to ArduPilot (velocity, position, etc.)
+
-- Publishes ROS 2 telemetry for the drone controller
+Usage:
+    from mavlink_bridge import MAVLinkCommander
+    
+    cmd = MAVLinkCommander()
+    cmd.connect()
+    cmd.arm()
+    cmd.takeoff(5.0)
+    cmd.land()
 """
 
-import json
-import math
 import time
 import threading
-from typing import Optional, Dict, Any, Tuple
+from typing import Optional, Callable
-
+from enum import Enum
-import rclpy
-from rclpy.node import Node
-from geometry_msgs.msg import Twist
-from std_msgs.msg import String
 
 try:
     from pymavlink import mavutil
-    from pymavlink.dialects.v20 import ardupilotmega as mavlink
+    PYMAVLINK_AVAILABLE = True
-    MAVLINK_AVAILABLE = True
 except ImportError:
-    MAVLINK_AVAILABLE = False
+    PYMAVLINK_AVAILABLE = False
-    print("WARNING: pymavlink not installed. Run: pip install pymavlink")
+    print("Warning: pymavlink not installed. Install with: pip install pymavlink")
 
 
-class MAVLinkBridge(Node):
+class FlightMode(Enum):
+    """ArduCopter flight modes."""
+    STABILIZE = 0
+    ACRO = 1
+    ALT_HOLD = 2
+    AUTO = 3
+    GUIDED = 4
+    LOITER = 5
+    RTL = 6
+    CIRCLE = 7
+    LAND = 9
+    DRIFT = 11
+    SPORT = 13
+    FLIP = 14
+    AUTOTUNE = 15
+    POSHOLD = 16
+    BRAKE = 17
+    THROW = 18
+    AVOID_ADSB = 19
+    GUIDED_NOGPS = 20
+    SMART_RTL = 21
+    FLOWHOLD = 22
+    FOLLOW = 23
+    ZIGZAG = 24
+    SYSTEMID = 25
+    AUTOROTATE = 26
+
+
+class MAVLinkCommander:
     """
-    ROS 2 Node that bridges between ROS topics and ArduPilot SITL via MAVLink.
+    MAVLink command interface for ArduPilot.
+    
+    Provides methods for arming, disarming, mode changes, and flight commands.
     """
     
-    # MAVLink connection defaults
+    def __init__(self, connection_string: str = "udpin:127.0.0.1:14550"):
-    DEFAULT_SITL_HOST = "127.0.0.1"
+        """
-    DEFAULT_SITL_PORT = 14550  # MAVProxy output port
+        Initialize MAVLink commander.
-    DEFAULT_SYSTEM_ID = 1
-    DEFAULT_COMPONENT_ID = 191  # MAV_COMP_ID_MISSIONPLANNER
         
-    # Telemetry rate (Hz)
+        Args:
-    TELEMETRY_RATE = 50.0
+            connection_string: MAVLink connection string (default for MAVProxy output)
+        """
+        if not PYMAVLINK_AVAILABLE:
+            raise ImportError("pymavlink is required: pip install pymavlink")
         
-    def __init__(
+        self._connection_string = connection_string
-        self,
+        self._mav: Optional[mavutil.mavlink_connection] = None
-        sitl_host: str = None,
-        sitl_port: int = None,
-        system_id: int = None,
-        component_id: int = None
-    ):
-        super().__init__('mavlink_bridge')
-        
-        # Connection parameters (with parameter overrides)
-        self.declare_parameter('sitl_host', sitl_host or self.DEFAULT_SITL_HOST)
-        self.declare_parameter('sitl_port', sitl_port or self.DEFAULT_SITL_PORT)
-        self.declare_parameter('system_id', system_id or self.DEFAULT_SYSTEM_ID)
-        self.declare_parameter('component_id', component_id or self.DEFAULT_COMPONENT_ID)
-        
-        self._sitl_host = self.get_parameter('sitl_host').value
-        self._sitl_port = self.get_parameter('sitl_port').value
-        self._system_id = self.get_parameter('system_id').value
-        self._component_id = self.get_parameter('component_id').value
-        
-        self.get_logger().info('=' * 60)
-        self.get_logger().info('MAVLink Bridge Starting...')
-        self.get_logger().info(f'  Connecting to SITL at {self._sitl_host}:{self._sitl_port}')
-        self.get_logger().info('=' * 60)
-        
-        # MAVLink connection
-        self._mav_conn: Optional[mavutil.mavlink_connection] = None
         self._connected = False
         self._armed = False
         self._flight_mode = "UNKNOWN"
+        self._heartbeat_thread: Optional[threading.Thread] = None
+        self._running = False
         
-        # Telemetry state
+        # Callbacks
-        self._position = [0.0, 0.0, 0.0]  # x, y, z (NED frame, converted to ENU)
+        self._on_heartbeat: Optional[Callable] = None
-        self._velocity = [0.0, 0.0, 0.0]  # vx, vy, vz
+        self._on_state_change: Optional[Callable] = None
-        self._attitude = [0.0, 0.0, 0.0]  # roll, pitch, yaw (radians)
-        self._angular_velocity = [0.0, 0.0, 0.0]  # wx, wy, wz
-        self._battery_voltage = 0.0
-        self._battery_remaining = 100
-        self._gps_fix = 0
-        self._landed = False
-        self._home_position = None
-        self._last_heartbeat = 0.0
     
-        # Rover position (for landing pad detection)
+    def connect(self, timeout: float = 30.0) -> bool:
-        self._rover_position = [0.0, 0.0, 0.15]
+        """
+        Connect to ArduPilot via MAVLink.
         
-        # Thread safety
+        Args:
-        self._lock = threading.Lock()
+            timeout: Connection timeout in seconds
             
-        # ROS 2 Subscriptions
+        Returns:
-        self._cmd_vel_sub = self.create_subscription(
+            True if connected successfully
-            Twist, '/cmd_vel', self._cmd_vel_callback, 10
+        """
-        )
+        print(f"Connecting to {self._connection_string}...")
-        self.get_logger().info('  Subscribed to: /cmd_vel')
-        
-        self._rover_sub = self.create_subscription(
-            String, '/rover/telemetry', self._rover_callback, 10
-        )
-        self.get_logger().info('  Subscribed to: /rover/telemetry')
-        
-        # ROS 2 Publishers
-        self._telemetry_pub = self.create_publisher(
-            String, '/drone/telemetry', 10
-        )
-        self.get_logger().info('  Publishing to: /drone/telemetry')
-        
-        self._status_pub = self.create_publisher(
-            String, '/mavlink/status', 10
-        )
-        self.get_logger().info('  Publishing to: /mavlink/status')
-        
-        # Initialize MAVLink connection
-        self._connect_mavlink()
-        
-        # Start telemetry thread
-        self._running = True
-        self._mavlink_thread = threading.Thread(target=self._mavlink_loop, daemon=True)
-        self._mavlink_thread.start()
-        
-        # Telemetry timer
-        telemetry_period = 1.0 / self.TELEMETRY_RATE
-        self._telemetry_timer = self.create_timer(telemetry_period, self._publish_telemetry)
-        
-        self.get_logger().info('MAVLink Bridge Ready!')
-
-    def _connect_mavlink(self) -> bool:
-        """Establish MAVLink connection to ArduPilot SITL."""
-        if not MAVLINK_AVAILABLE:
-            self.get_logger().error('pymavlink not available!')
-            return False
         
         try:
-            connection_string = f'udpin:{self._sitl_host}:{self._sitl_port}'
+            self._mav = mavutil.mavlink_connection(self._connection_string)
-            self.get_logger().info(f'Connecting to: {connection_string}')
-            
-            self._mav_conn = mavutil.mavlink_connection(
-                connection_string,
-                source_system=self._system_id,
-                source_component=self._component_id
-            )
             
             # Wait for heartbeat
-            self.get_logger().info('Waiting for heartbeat...')
+            heartbeat = self._mav.wait_heartbeat(timeout=timeout)
-            self._mav_conn.wait_heartbeat(timeout=30)
+            if heartbeat is None:
+                print("Connection failed: No heartbeat received")
+                return False
             
             self._connected = True
-            self._last_heartbeat = time.time()
+            self._running = True
-            self.get_logger().info(
+            
-                f'Connected! System: {self._mav_conn.target_system}, '
+            # Start heartbeat thread
-                f'Component: {self._mav_conn.target_component}'
+            self._heartbeat_thread = threading.Thread(
+                target=self._heartbeat_loop, daemon=True
             )
+            self._heartbeat_thread.start()
             
-            # Request data streams
+            print(f"Connected! System: {self._mav.target_system}, Component: {self._mav.target_component}")
-            self._request_data_streams()
-            
             return True
             
         except Exception as e:
-            self.get_logger().error(f'MAVLink connection failed: {e}')
+            print(f"Connection failed: {e}")
-            self._connected = False
             return False
     
-    def _request_data_streams(self):
+    def disconnect(self):
-        """Request required data streams from ArduPilot."""
+        """Disconnect from ArduPilot."""
-        if not self._mav_conn:
+        self._running = False
-            return
+        if self._heartbeat_thread:
-        
+            self._heartbeat_thread.join(timeout=2.0)
-        # Request all data streams at high rate
+        if self._mav:
-        streams = [
+            self._mav.close()
-            (mavutil.mavlink.MAV_DATA_STREAM_ALL, 10),
+        self._connected = False
-            (mavutil.mavlink.MAV_DATA_STREAM_RAW_SENSORS, 50),
+        print("Disconnected")
-            (mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, 5),
-            (mavutil.mavlink.MAV_DATA_STREAM_POSITION, 50),
-            (mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, 50),  # Attitude
-            (mavutil.mavlink.MAV_DATA_STREAM_EXTRA2, 10),  # VFR_HUD
-        ]
-        
-        for stream_id, rate in streams:
-            self._mav_conn.mav.request_data_stream_send(
-                self._mav_conn.target_system,
-                self._mav_conn.target_component,
-                stream_id,
-                rate,
-                1  # Start
-            )
-
-    def _mavlink_loop(self):
-        """Background thread for receiving MAVLink messages."""
-        reconnect_delay = 2.0
-        
-        while self._running:
-            if not self._connected:
-                time.sleep(reconnect_delay)
-                self._connect_mavlink()
-                continue
     
+    def _heartbeat_loop(self):
+        """Background thread for receiving heartbeats and updating state."""
+        while self._running and self._mav:
             try:
-                # Non-blocking message receive
+                msg = self._mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1.0)
-                msg = self._mav_conn.recv_match(blocking=True, timeout=0.1)
+                if msg:
-                
-                if msg is None:
-                    # Check for connection timeout
-                    if time.time() - self._last_heartbeat > 5.0:
-                        self.get_logger().warning('Heartbeat timeout, reconnecting...')
-                        self._connected = False
-                    continue
-                
-                self._process_mavlink_message(msg)
-                
-            except Exception as e:
-                self.get_logger().error(f'MAVLink receive error: {e}')
-                self._connected = False
-
-    def _process_mavlink_message(self, msg):
-        """Process incoming MAVLink messages."""
-        msg_type = msg.get_type()
-        
-        with self._lock:
-            if msg_type == 'HEARTBEAT':
-                self._last_heartbeat = time.time()
                     self._armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
                     self._flight_mode = mavutil.mode_string_v10(msg)
                     
-            elif msg_type == 'LOCAL_POSITION_NED':
+                    if self._on_heartbeat:
-                # Convert NED to ENU (ROS convention)
+                        self._on_heartbeat(self._armed, self._flight_mode)
-                self._position = [msg.y, msg.x, -msg.z]  # NED to ENU
+            except Exception:
-                self._velocity = [msg.vy, msg.vx, -msg.vz]
-                
-            elif msg_type == 'GLOBAL_POSITION_INT':
-                # Backup position from GPS
-                if self._home_position is None:
-                    self._home_position = (msg.lat, msg.lon, msg.alt)
-                
-            elif msg_type == 'ATTITUDE':
-                self._attitude = [msg.roll, msg.pitch, msg.yaw]
-                self._angular_velocity = [msg.rollspeed, msg.pitchspeed, msg.yawspeed]
-                
-            elif msg_type == 'SYS_STATUS':
-                self._battery_voltage = msg.voltage_battery / 1000.0  # mV to V
-                self._battery_remaining = msg.battery_remaining
-                
-            elif msg_type == 'GPS_RAW_INT':
-                self._gps_fix = msg.fix_type
-                
-            elif msg_type == 'EXTENDED_SYS_STATE':
-                # Check landed state
-                self._landed = (msg.landed_state == mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND)
-
-    def _cmd_vel_callback(self, msg: Twist):
-        """Handle velocity commands from drone controller."""
-        if not self._connected or not self._mav_conn:
-            return
-        
-        try:
-            # Convert ROS Twist to MAVLink velocity command
-            # linear.x = forward (pitch), linear.y = left (roll), linear.z = up (thrust)
-            # angular.z = yaw rate
-            
-            # Use SET_POSITION_TARGET_LOCAL_NED for velocity control
-            # Type mask: velocity only (ignore position and acceleration)
-            type_mask = (
-                mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
-                mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
-                mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
-                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
-                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
-                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
-                mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE
-            )
-            
-            # Convert velocities (ENU to NED)
-            vx = msg.linear.x  # Forward (body frame pitch command)
-            vy = msg.linear.y  # Left (body frame roll command)
-            vz = -msg.linear.z  # Down (negative because NED)
-            yaw_rate = msg.angular.z
-            
-            self._mav_conn.mav.set_position_target_local_ned_send(
-                0,  # time_boot_ms (not used)
-                self._mav_conn.target_system,
-                self._mav_conn.target_component,
-                mavutil.mavlink.MAV_FRAME_LOCAL_NED,
-                type_mask,
-                0, 0, 0,  # position (ignored)
-                vx, vy, vz,  # velocity
-                0, 0, 0,  # acceleration (ignored)
-                0, yaw_rate  # yaw, yaw_rate
-            )
-            
-        except Exception as e:
-            self.get_logger().warning(f'Failed to send velocity command: {e}')
-
-    def _rover_callback(self, msg: String):
-        """Receive rover position for landing pad detection."""
-        try:
-            data = json.loads(msg.data)
-            pos = data.get('position', {})
-            with self._lock:
-                self._rover_position = [
-                    pos.get('x', 0.0),
-                    pos.get('y', 0.0),
-                    pos.get('z', 0.15)
-                ]
-        except json.JSONDecodeError:
                 pass
     
-    def _get_landing_pad_detection(self) -> Optional[Dict[str, float]]:
+    @property
-        """Simulate landing pad detection based on relative position."""
+    def connected(self) -> bool:
-        # Camera parameters
+        return self._connected
-        CAMERA_FOV = 60.0
-        CAMERA_RANGE = 10.0
     
-        dx = self._rover_position[0] - self._position[0]
+    @property
-        dy = self._rover_position[1] - self._position[1]
+    def armed(self) -> bool:
-        dz = self._rover_position[2] - self._position[2]
+        return self._armed
     
-        horizontal_dist = math.sqrt(dx**2 + dy**2)
+    @property
-        vertical_dist = -dz  # Height above rover
+    def flight_mode(self) -> str:
-        
+        return self._flight_mode
-        if vertical_dist <= 0 or vertical_dist > CAMERA_RANGE:
-            return None
-        
-        fov_rad = math.radians(CAMERA_FOV / 2)
-        max_horizontal = vertical_dist * math.tan(fov_rad)
-        
-        if horizontal_dist > max_horizontal:
-            return None
-        
-        # Transform to body frame using yaw
-        yaw = self._attitude[2]
-        cos_yaw = math.cos(-yaw)
-        sin_yaw = math.sin(-yaw)
-        relative_x = dx * cos_yaw - dy * sin_yaw
-        relative_y = dx * sin_yaw + dy * cos_yaw
-        
-        # Calculate confidence
-        angle = math.atan2(horizontal_dist, vertical_dist)
-        confidence = max(0.0, 1.0 - (angle / fov_rad))
-        confidence *= max(0.0, 1.0 - (vertical_dist / CAMERA_RANGE))
-        
-        return {
-            "relative_x": round(relative_x, 4),
-            "relative_y": round(relative_y, 4),
-            "distance": round(vertical_dist, 4),
-            "confidence": round(confidence, 4)
-        }
-
-    def _publish_telemetry(self):
-        """Publish drone telemetry to ROS 2 topic."""
-        with self._lock:
-            # Landing pad detection
-            pad_detection = self._get_landing_pad_detection()
-            
-            telemetry = {
-                "imu": {
-                    "orientation": {
-                        "roll": round(self._attitude[0], 4),
-                        "pitch": round(self._attitude[1], 4),
-                        "yaw": round(self._attitude[2], 4)
-                    },
-                    "angular_velocity": {
-                        "x": round(self._angular_velocity[0], 4),
-                        "y": round(self._angular_velocity[1], 4),
-                        "z": round(self._angular_velocity[2], 4)
-                    },
-                    "linear_acceleration": {
-                        "x": 0.0,
-                        "y": 0.0,
-                        "z": 9.81
-                    }
-                },
-                "altimeter": {
-                    "altitude": round(self._position[2], 4),
-                    "vertical_velocity": round(self._velocity[2], 4)
-                },
-                "velocity": {
-                    "x": round(self._velocity[0], 4),
-                    "y": round(self._velocity[1], 4),
-                    "z": round(self._velocity[2], 4)
-                },
-                "position": {
-                    "x": round(self._position[0], 4),
-                    "y": round(self._position[1], 4),
-                    "z": round(self._position[2], 4)
-                },
-                "landing_pad": pad_detection,
-                "camera": {
-                    "width": 320,
-                    "height": 240,
-                    "fov": 60.0,
-                    "image": None  # Would need camera integration
-                },
-                "battery": {
-                    "voltage": round(self._battery_voltage, 2),
-                    "remaining": self._battery_remaining
-                },
-                "landed": self._landed,
-                "armed": self._armed,
-                "flight_mode": self._flight_mode,
-                "connected": self._connected,
-                "timestamp": round(time.time(), 4)
-            }
-        
-        telemetry_msg = String()
-        telemetry_msg.data = json.dumps(telemetry)
-        self._telemetry_pub.publish(telemetry_msg)
-        
-        # Publish status
-        status_msg = String()
-        status_msg.data = json.dumps({
-            "connected": self._connected,
-            "armed": self._armed,
-            "mode": self._flight_mode,
-            "gps_fix": self._gps_fix
-        })
-        self._status_pub.publish(status_msg)
-
-    # =========================================================================
-    # ArduPilot Control Commands
-    # =========================================================================
     
     def arm(self, force: bool = False) -> bool:
-        """Arm the drone."""
+        """
-        if not self._connected or not self._mav_conn:
+        Arm the vehicle.
+        
+        Args:
+            force: Force arm (bypass preflight checks)
+            
+        Returns:
+            True if arm command was sent
+        """
+        if not self._connected:
+            print("Not connected")
             return False
         
-        self.get_logger().info('Arming drone...')
+        print("Arming...")
-        self._mav_conn.arducopter_arm()
+        
-        return self._wait_for_arm(True, timeout=5.0)
+        param = 21196.0 if force else 0.0  # Force arm magic number
+        
+        self._mav.mav.command_long_send(
+            self._mav.target_system,
+            self._mav.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0,  # Confirmation
+            1,  # Arm
+            param,  # Force
+            0, 0, 0, 0, 0
+        )
+        
+        # Wait for confirmation
+        time.sleep(0.5)
+        return True
     
     def disarm(self, force: bool = False) -> bool:
-        """Disarm the drone."""
+        """
-        if not self._connected or not self._mav_conn:
+        Disarm the vehicle.
+        
+        Args:
+            force: Force disarm
+            
+        Returns:
+            True if disarm command was sent
+        """
+        if not self._connected:
+            print("Not connected")
             return False
         
-        self.get_logger().info('Disarming drone...')
+        print("Disarming...")
-        self._mav_conn.arducopter_disarm()
-        return self._wait_for_arm(False, timeout=5.0)
         
-    def _wait_for_arm(self, expected: bool, timeout: float = 5.0) -> bool:
+        param = 21196.0 if force else 0.0
-        """Wait for arm/disarm to complete."""
+        
-        start = time.time()
+        self._mav.mav.command_long_send(
-        while time.time() - start < timeout:
+            self._mav.target_system,
-            if self._armed == expected:
+            self._mav.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0,
+            0,  # Disarm
+            param,
+            0, 0, 0, 0, 0
+        )
+        
+        time.sleep(0.5)
         return True
-            time.sleep(0.1)
-        return False
     
     def set_mode(self, mode: str) -> bool:
-        """Set flight mode (e.g., 'GUIDED', 'LAND', 'LOITER')."""
+        """
-        if not self._connected or not self._mav_conn:
+        Set flight mode.
+        
+        Args:
+            mode: Mode name (GUIDED, LAND, LOITER, etc.)
+            
+        Returns:
+            True if mode change command was sent
+        """
+        if not self._connected:
+            print("Not connected")
             return False
         
-        self.get_logger().info(f'Setting mode to: {mode}')
+        mode_upper = mode.upper()
+        mode_id = self._mav.mode_mapping().get(mode_upper)
         
-        mode_mapping = self._mav_conn.mode_mapping()
+        if mode_id is None:
-        if mode.upper() not in mode_mapping:
+            print(f"Unknown mode: {mode}")
-            self.get_logger().error(f'Unknown mode: {mode}')
             return False
         
-        mode_id = mode_mapping[mode.upper()]
+        print(f"Setting mode to {mode_upper}...")
-        self._mav_conn.set_mode(mode_id)
         
-        # Wait for mode change
+        self._mav.mav.set_mode_send(
-        start = time.time()
+            self._mav.target_system,
-        while time.time() - start < 5.0:
+            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
-            if self._flight_mode.upper() == mode.upper():
+            mode_id
+        )
+        
+        time.sleep(0.5)
         return True
-            time.sleep(0.1)
-        
-        return False
     
     def takeoff(self, altitude: float = 5.0) -> bool:
-        """Take off to specified altitude."""
+        """
-        if not self._connected or not self._mav_conn:
+        Command takeoff to specified altitude.
+        
+        Args:
+            altitude: Target altitude in meters
+            
+        Returns:
+            True if takeoff command was sent
+        """
+        if not self._connected:
+            print("Not connected")
             return False
         
-        self.get_logger().info(f'Taking off to {altitude}m...')
+        print(f"Taking off to {altitude}m...")
         
-        # Set GUIDED mode
+        self._mav.mav.command_long_send(
-        if not self.set_mode('GUIDED'):
+            self._mav.target_system,
-            self.get_logger().error('Failed to set GUIDED mode')
+            self._mav.target_component,
-            return False
-        
-        # Arm if not armed
-        if not self._armed:
-            if not self.arm():
-                self.get_logger().error('Failed to arm')
-                return False
-        
-        # Command takeoff
-        self._mav_conn.mav.command_long_send(
-            self._mav_conn.target_system,
-            self._mav_conn.target_component,
             mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
-            0,  # confirmation
+            0,
-            0, 0, 0, 0,  # params 1-4 (unused)
+            0, 0, 0, 0, 0, 0,
-            0, 0,  # lat, lon (use current)
+            altitude
-            altitude  # altitude
         )
         
         return True
     
     def land(self) -> bool:
-        """Command the drone to land."""
+        """
-        if not self._connected or not self._mav_conn:
+        Command landing.
+        
+        Returns:
+            True if land command was sent
+        """
+        if not self._connected:
+            print("Not connected")
             return False
         
-        self.get_logger().info('Landing...')
+        print("Landing...")
-        return self.set_mode('LAND')
         
-    def goto_position(self, x: float, y: float, z: float, yaw: float = 0.0):
+        self._mav.mav.command_long_send(
-        """Go to a local position (ENU frame)."""
+            self._mav.target_system,
-        if not self._connected or not self._mav_conn:
+            self._mav.target_component,
-            return
+            mavutil.mavlink.MAV_CMD_NAV_LAND,
-        
+            0,
-        # Convert ENU to NED
+            0, 0, 0, 0, 0, 0, 0
-        type_mask = (
-            mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE |
-            mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |
-            mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE |
-            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
-            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
-            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
-            mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
         )
         
-        self._mav_conn.mav.set_position_target_local_ned_send(
+        return True
+    
+    def goto_position_ned(self, north: float, east: float, down: float) -> bool:
+        """
+        Go to position in NED frame relative to home.
+        
+        Args:
+            north: North position (m)
+            east: East position (m)
+            down: Down position (m, negative is up)
+            
+        Returns:
+            True if command was sent
+        """
+        if not self._connected:
+            print("Not connected")
+            return False
+        
+        print(f"Going to NED: ({north}, {east}, {down})")
+        
+        self._mav.mav.set_position_target_local_ned_send(
             0,  # time_boot_ms
-            self._mav_conn.target_system,
+            self._mav.target_system,
-            self._mav_conn.target_component,
+            self._mav.target_component,
             mavutil.mavlink.MAV_FRAME_LOCAL_NED,
-            type_mask,
+            0b0000111111111000,  # Position only
-            y, x, -z,  # ENU to NED position
+            north, east, down,  # Position
-            0, 0, 0,  # velocity (ignored)
+            0, 0, 0,  # Velocity
-            0, 0, 0,  # acceleration (ignored)
+            0, 0, 0,  # Acceleration
-            yaw, 0  # yaw, yaw_rate
+            0, 0  # Yaw, yaw_rate
         )
         
-    def destroy_node(self):
+        return True
-        """Clean shutdown."""
+    
-        self._running = False
+    def set_velocity_ned(self, vn: float, ve: float, vd: float, yaw_rate: float = 0.0) -> bool:
-        if self._mavlink_thread.is_alive():
+        """
-            self._mavlink_thread.join(timeout=2.0)
+        Set velocity in NED frame.
-        if self._mav_conn:
+        
-            self._mav_conn.close()
+        Args:
-        super().destroy_node()
+            vn: North velocity (m/s)
+            ve: East velocity (m/s)
+            vd: Down velocity (m/s, positive is down)
+            yaw_rate: Yaw rate (rad/s)
+            
+        Returns:
+            True if command was sent
+        """
+        if not self._connected:
+            return False
+        
+        self._mav.mav.set_position_target_local_ned_send(
+            0,  # time_boot_ms
+            self._mav.target_system,
+            self._mav.target_component,
+            mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+            0b0000011111000111,  # Velocity only
+            0, 0, 0,  # Position (ignored)
+            vn, ve, vd,  # Velocity
+            0, 0, 0,  # Acceleration
+            0, yaw_rate
+        )
+        
+        return True
+    
+    def set_param(self, param_id: str, value: float) -> bool:
+        """
+        Set a parameter value.
+        
+        Args:
+            param_id: Parameter name
+            value: Parameter value
+            
+        Returns:
+            True if command was sent
+        """
+        if not self._connected:
+            print("Not connected")
+            return False
+        
+        print(f"Setting {param_id} = {value}")
+        
+        self._mav.mav.param_set_send(
+            self._mav.target_system,
+            self._mav.target_component,
+            param_id.encode('utf-8'),
+            value,
+            mavutil.mavlink.MAV_PARAM_TYPE_REAL32
+        )
+        
+        return True
 
 
-def main(args=None):
+def main():
-    if not MAVLINK_AVAILABLE:
+    """Example usage of MAVLinkCommander."""
-        print("ERROR: pymavlink is required. Install with: pip install pymavlink")
+    if not PYMAVLINK_AVAILABLE:
+        print("pymavlink not installed")
         return
     
-    print("\n" + "=" * 60)
+    cmd = MAVLinkCommander()
-    print("  MAVLink Bridge for ArduPilot SITL")
-    print("=" * 60 + "\n")
     
-    rclpy.init(args=args)
+    if not cmd.connect(timeout=10.0):
-    bridge = None
+        print("Failed to connect")
+        return
     
-    try:
+    print(f"Armed: {cmd.armed}")
-        bridge = MAVLinkBridge()
+    print(f"Mode: {cmd.flight_mode}")
-        rclpy.spin(bridge)
+    
-    except KeyboardInterrupt:
+    # Example commands (uncomment to use):
-        print('\nShutting down...')
+    # cmd.set_mode("GUIDED")
-    finally:
+    # cmd.arm()
-        if bridge:
+    # cmd.takeoff(5.0)
-            bridge.destroy_node()
+    # time.sleep(10)
-        if rclpy.ok():
+    # cmd.land()
-            rclpy.shutdown()
+    
+    input("Press Enter to disconnect...")
+    cmd.disconnect()
 
 
 if __name__ == '__main__':

requirements.txt

@@ -1,5 +1,16 @@
+# Drone Simulation Requirements
+# Install with: pip install -r requirements.txt
+
+# Core simulation
 pybullet>=3.2.5
 numpy>=1.20.0
 pillow>=9.0.0
-pyinstaller>=6.0.0
+
+# Camera viewer
+opencv-python>=4.5.0
+
+# ArduPilot/MAVLink (optional - for ArduPilot mode)
 pymavlink>=2.4.0
+
+# Build executables (optional)
+pyinstaller>=6.0.0

run_ardupilot.py

@@ -1,16 +1,20 @@
 #!/usr/bin/env python3
 """
-Run ArduPilot SITL + Gazebo Simulation.
+ArduPilot ROS 2 Launcher - Official DDS Integration.
 
-This script orchestrates the full ArduPilot SITL + Gazebo simulation stack:
+This script provides a convenient way to launch the ArduPilot SITL simulation
-1. Launches ArduPilot SITL (sim_vehicle.py with Gazebo model)
+using the official ardupilot_gz packages with ROS 2 DDS support.
-2. Starts MAVProxy for GCS connectivity
-3. Runs MAVLink bridge + controllers
 
 Usage:
-    python run_ardupilot.py                    # Start everything
+    python run_ardupilot.py                    # Launch Iris on runway
-    python run_ardupilot.py --pattern circular # With rover movement
+    python run_ardupilot.py --world maze       # Launch Iris in maze
-    python run_ardupilot.py --no-sitl          # Just controllers (SITL already running)
+    python run_ardupilot.py --vehicle rover    # Launch WildThumper rover
+    python run_ardupilot.py --mavproxy         # Also start MAVProxy
+
+Prerequisites:
+    - ArduPilot ROS 2 packages installed (./setup/install_ardupilot.sh)
+    - ROS 2 Humble/Jazzy sourced
+    - ~/ardu_ws workspace built and sourced
 """
 
 import argparse
@@ -21,336 +25,208 @@ import sys
 import time
 from pathlib import Path
 
-import rclpy
-from rclpy.executors import MultiThreadedExecutor
 
-# Check for pymavlink
+def check_ros2():
+    """Check if ROS 2 is available."""
     try:
-    from pymavlink import mavutil
+        subprocess.run(['ros2', '--help'], capture_output=True, check=True)
-    MAVLINK_AVAILABLE = True
+        return True
-except ImportError:
+    except (subprocess.CalledProcessError, FileNotFoundError):
-    MAVLINK_AVAILABLE = False
+        return False
-
-from mavlink_bridge import MAVLinkBridge
-from drone_controller import DroneController
-from rover_controller import RoverController, MovementPattern
 
 
-def find_sim_vehicle() -> str:
+def check_ardupilot_packages():
-    """Find sim_vehicle.py in common ArduPilot locations."""
+    """Check if ArduPilot ROS 2 packages are installed."""
-    locations = [
+    try:
-        os.path.expanduser("~/ardupilot/Tools/autotest/sim_vehicle.py"),
+        result = subprocess.run(
-        "/opt/ardupilot/Tools/autotest/sim_vehicle.py",
+            ['ros2', 'pkg', 'list'],
-        os.environ.get("ARDUPILOT_HOME", "") + "/Tools/autotest/sim_vehicle.py",
+            capture_output=True,
+            text=True,
+            check=True
+        )
+        packages = result.stdout
+        return 'ardupilot_gz_bringup' in packages or 'ardupilot_sitl' in packages
+    except (subprocess.CalledProcessError, FileNotFoundError):
+        return False
+
+
+def source_workspace():
+    """Source the ArduPilot workspace."""
+    ardu_ws = os.path.expanduser("~/ardu_ws")
+    setup_bash = os.path.join(ardu_ws, "install", "setup.bash")
+    
+    if os.path.exists(setup_bash):
+        # Update environment by sourcing the workspace
+        # This is done by running commands in a sourced shell
+        return True
+    return False
+
+
+def get_launch_command(world: str, vehicle: str) -> list:
+    """Get the appropriate launch command."""
+    
+    launch_files = {
+        # Copter configurations
+        'runway': ('ardupilot_gz_bringup', 'iris_runway.launch.py'),
+        'maze': ('ardupilot_gz_bringup', 'iris_maze.launch.py'),
+        'iris': ('ardupilot_gz_bringup', 'iris_runway.launch.py'),
+        
+        # Rover configurations
+        'rover': ('ardupilot_gz_bringup', 'wildthumper_playpen.launch.py'),
+        'wildthumper': ('ardupilot_gz_bringup', 'wildthumper_playpen.launch.py'),
+        
+        # SITL only (no Gazebo)
+        'sitl': ('ardupilot_sitl', 'sitl_dds_udp.launch.py'),
+    }
+    
+    if vehicle == 'rover':
+        key = 'rover'
+    else:
+        key = world if world in launch_files else 'runway'
+    
+    package, launch_file = launch_files.get(key, launch_files['runway'])
+    
+    cmd = ['ros2', 'launch', package, launch_file]
+    
+    # Add SITL-specific parameters if using sitl_dds_udp
+    if launch_file == 'sitl_dds_udp.launch.py':
+        cmd.extend([
+            'transport:=udp4',
+            'synthetic_clock:=True',
+            'model:=quad' if vehicle == 'copter' else 'rover',
+            'speedup:=1',
+        ])
+    
+    return cmd
+
+
+def launch_mavproxy(master_port: int = 14550):
+    """Launch MAVProxy in a new terminal."""
+    mavproxy_cmd = f"mavproxy.py --console --map --master=:{master_port}"
+    
+    # Try different terminal emulators
+    terminals = [
+        ['gnome-terminal', '--', 'bash', '-c', mavproxy_cmd],
+        ['xterm', '-e', mavproxy_cmd],
+        ['konsole', '-e', mavproxy_cmd],
     ]
     
-    for loc in locations:
+    for term_cmd in terminals:
-        if os.path.exists(loc):
+        try:
-            return loc
+            subprocess.Popen(term_cmd)
+            return True
+        except FileNotFoundError:
+            continue
     
-    # Try finding it in PATH
+    print(f"[WARN] Could not open terminal for MAVProxy")
-    import shutil
+    print(f"[INFO] Run manually: {mavproxy_cmd}")
-    if shutil.which("sim_vehicle.py"):
+    return False
-        return "sim_vehicle.py"
-    
-    return None
 
 
 def parse_args():
     parser = argparse.ArgumentParser(
-        description='Run ArduPilot SITL + Gazebo + MAVLink Bridge',
+        description='Launch ArduPilot SITL with ROS 2 and Gazebo',
         formatter_class=argparse.RawDescriptionHelpFormatter,
         epilog="""
 Examples:
-  python run_ardupilot.py                       # Full stack with Gazebo
+  python run_ardupilot.py                  # Iris on runway
-  python run_ardupilot.py --pattern circular    # Moving rover
+  python run_ardupilot.py --world maze     # Iris in maze
-  python run_ardupilot.py --no-sitl             # SITL already running
+  python run_ardupilot.py --vehicle rover  # WildThumper rover
-  python run_ardupilot.py --mavproxy-port 14551 # Custom port
+  python run_ardupilot.py --mavproxy       # With MAVProxy
+
+Available worlds: runway, maze, sitl (no Gazebo)
+Available vehicles: copter, rover
         """
     )
     
-    # SITL options
     parser.add_argument(
-        '--no-sitl', action='store_true',
+        '--world', '-w', type=str, default='runway',
-        help='Skip launching SITL (assume already running)'
+        choices=['runway', 'maze', 'sitl'],
+        help='Simulation world (default: runway)'
     )
     parser.add_argument(
-        '--sitl-path', type=str,
+        '--vehicle', '-v', type=str, default='copter',
-        help='Path to sim_vehicle.py'
+        choices=['copter', 'rover'],
+        help='Vehicle type (default: copter)'
     )
     parser.add_argument(
-        '--vehicle', type=str, default='ArduCopter',
+        '--mavproxy', '-m', action='store_true',
-        choices=['ArduCopter', 'ArduPlane', 'APMrover2'],
+        help='Also launch MAVProxy in a new terminal'
-        help='Vehicle type (default: ArduCopter)'
     )
-    parser.add_argument(
-        '--frame', type=str, default='gazebo-iris',
-        help='Vehicle frame (default: gazebo-iris)'
-    )
-    
-    # MAVProxy settings
     parser.add_argument(
         '--mavproxy-port', type=int, default=14550,
-        help='MAVProxy output port (default: 14550)'
+        help='MAVProxy master port (default: 14550)'
-    )
-    parser.add_argument(
-        '--sitl-port', type=int, default=5760,
-        help='SITL connection port (default: 5760)'
     )
     
-    # Rover options
+    return parser.parse_args()
-    parser.add_argument(
-        '--pattern', type=str, default='stationary',
-        choices=['stationary', 'linear', 'circular', 'random', 'square'],
-        help='Rover movement pattern (default: stationary)'
-    )
-    parser.add_argument(
-        '--speed', '-s', type=float, default=0.5,
-        help='Rover speed in m/s (default: 0.5)'
-    )
-    parser.add_argument(
-        '--amplitude', '-a', type=float, default=2.0,
-        help='Rover movement amplitude in meters (default: 2.0)'
-    )
-    parser.add_argument(
-        '--no-rover', action='store_true',
-        help='Disable rover controller'
-    )
-    
-    # Other
-    parser.add_argument(
-        '--console', action='store_true',
-        help='Show MAVProxy console'
-    )
-    parser.add_argument(
-        '--map', action='store_true',
-        help='Show MAVProxy map'
-    )
-    
-    args, _ = parser.parse_known_args()
-    return args
-
-
-class SITLProcess:
-    """Manages the ArduPilot SITL process."""
-    
-    def __init__(
-        self,
-        sim_vehicle_path: str,
-        vehicle: str = 'ArduCopter',
-        frame: str = 'gazebo-iris',
-        sitl_port: int = 5760,
-        mavproxy_port: int = 14550,
-        console: bool = False,
-        show_map: bool = False
-    ):
-        self.sim_vehicle_path = sim_vehicle_path
-        self.vehicle = vehicle
-        self.frame = frame
-        self.sitl_port = sitl_port
-        self.mavproxy_port = mavproxy_port
-        self.console = console
-        self.show_map = show_map
-        self.process = None
-    
-    def start(self) -> bool:
-        """Start SITL process."""
-        if not self.sim_vehicle_path:
-            print("ERROR: sim_vehicle.py not found!")
-            print("Please set --sitl-path or ARDUPILOT_HOME environment variable")
-            return False
-        
-        cmd = [
-            sys.executable, self.sim_vehicle_path,
-            '-v', self.vehicle,
-            '-f', self.frame,
-            '--model', 'JSON',
-            '--sitl-instance-args', f'--sim-address=127.0.0.1',
-            '--out', f'udp:127.0.0.1:{self.mavproxy_port}',
-        ]
-        
-        if self.console:
-            cmd.append('--console')
-        if self.show_map:
-            cmd.append('--map')
-        
-        print(f"Starting SITL: {' '.join(cmd)}")
-        
-        try:
-            self.process = subprocess.Popen(
-                cmd,
-                stdout=subprocess.PIPE,
-                stderr=subprocess.STDOUT
-            )
-            return True
-        except Exception as e:
-            print(f"Failed to start SITL: {e}")
-            return False
-    
-    def stop(self):
-        """Stop SITL process."""
-        if self.process:
-            self.process.terminate()
-            self.process.wait(timeout=5.0)
-            self.process = None
-
-
-def wait_for_sitl(host: str = '127.0.0.1', port: int = 14550, timeout: float = 60.0) -> bool:
-    """Wait for SITL to be ready."""
-    print(f"Waiting for SITL on {host}:{port}...")
-    start = time.time()
-    
-    while time.time() - start < timeout:
-        try:
-            conn = mavutil.mavlink_connection(f'udpin:{host}:{port}', timeout=1)
-            if conn.wait_heartbeat(timeout=1):
-                print("SITL is ready!")
-                conn.close()
-                return True
-        except Exception:
-            pass
-        time.sleep(1)
-    
-    print("Timeout waiting for SITL")
-    return False
 
 
 def main():
     args = parse_args()
     
-    if not MAVLINK_AVAILABLE:
+    print("=" * 60)
-        print("ERROR: pymavlink is required!")
+    print("  ArduPilot SITL + Gazebo (Official ROS 2 DDS)")
-        print("Install with: pip install pymavlink")
+    print("=" * 60)
+    print()
+    
+    # Check ROS 2
+    if not check_ros2():
+        print("[ERROR] ROS 2 not found!")
+        print("Please source ROS 2:")
+        print("  source /opt/ros/humble/setup.bash")
         return 1
     
-    print("=" * 60)
+    print("[OK] ROS 2 available")
-    print("  ArduPilot SITL + Gazebo Simulation")
+    
-    print("=" * 60)
+    # Check ArduPilot packages
+    if not check_ardupilot_packages():
+        print("[ERROR] ArduPilot ROS 2 packages not found!")
+        print()
+        print("Please install ArduPilot ROS 2:")
+        print("  ./setup/install_ardupilot.sh")
+        print()
+        print("Then source the workspace:")
+        print("  source ~/ardu_ws/install/setup.bash")
+        return 1
+    
+    print("[OK] ArduPilot ROS 2 packages found")
+    
+    # Get launch command
+    launch_cmd = get_launch_command(args.world, args.vehicle)
+    
+    print()
+    print(f"World: {args.world}")
     print(f"Vehicle: {args.vehicle}")
-    print(f"  Frame: {args.frame}")
+    print(f"Launch: {' '.join(launch_cmd)}")
-    print(f"  Rover Pattern: {args.pattern}")
-    print(f"  MAVProxy Port: {args.mavproxy_port}")
-    print("=" * 60)
     print()
     
-    sitl = None
+    # Launch MAVProxy if requested
+    if args.mavproxy:
+        print("[INFO] Starting MAVProxy...")
+        # Delay to let SITL start first
+        time.sleep(2)
+        launch_mavproxy(args.mavproxy_port)
     
-    # Start SITL if not skipped
+    print("Starting simulation...")
-    if not args.no_sitl:
-        sim_vehicle = args.sitl_path or find_sim_vehicle()
-        
-        if not sim_vehicle:
-            print("=" * 60)
-            print("WARNING: sim_vehicle.py not found!")
-            print()
-            print("Please either:")
-            print("1. Install ArduPilot: https://ardupilot.org/dev/docs/building-setup-linux-ubuntu-apt.html")
-            print("2. Set ARDUPILOT_HOME environment variable")
-            print("3. Use --sitl-path to specify location")
-            print("4. Use --no-sitl if SITL is already running")
-            print("=" * 60)
-            print()
-            print("Continuing without starting SITL...")
-        else:
-            sitl = SITLProcess(
-                sim_vehicle,
-                vehicle=args.vehicle,
-                frame=args.frame,
-                sitl_port=args.sitl_port,
-                mavproxy_port=args.mavproxy_port,
-                console=args.console,
-                show_map=args.map
-            )
-            
-            if not sitl.start():
-                return 1
-            
-            print("\nWaiting for SITL to initialize...")
-            time.sleep(5)  # Give SITL time to start
-    
-    # Wait for SITL connection
-    if not wait_for_sitl(port=args.mavproxy_port, timeout=30):
-        print("Could not connect to SITL!")
-        if sitl:
-            sitl.stop()
-        return 1
-    
-    # Initialize ROS
-    rclpy.init()
-    
-    nodes = []
-    executor = MultiThreadedExecutor(num_threads=4)
-    
-    try:
-        # Create MAVLink bridge
-        bridge = MAVLinkBridge(
-            sitl_port=args.mavproxy_port
-        )
-        nodes.append(bridge)
-        executor.add_node(bridge)
-        print("[OK] MAVLink Bridge started")
-        
-        # Create drone controller
-        drone = DroneController()
-        nodes.append(drone)
-        executor.add_node(drone)
-        print("[OK] Drone Controller started")
-        
-        # Create rover controller
-        if not args.no_rover:
-            pattern = MovementPattern[args.pattern.upper()]
-            rover = RoverController(
-                pattern=pattern,
-                speed=args.speed,
-                amplitude=args.amplitude
-            )
-            nodes.append(rover)
-            executor.add_node(rover)
-            print("[OK] Rover Controller started")
-        
-        print()
-        print("=" * 60)
-        print("System ready!")
-        print()
-        print("To arm and takeoff, use MAVProxy commands:")
-        print("  mode guided")
-        print("  arm throttle")
-        print("  takeoff 5")
-        print()
-        print("Or use the bridge.arm() and bridge.takeoff() methods")
-        print("=" * 60)
-        print()
     print("Press Ctrl+C to stop.")
     print()
+    print("-" * 60)
     
-        # Handle Ctrl+C
+    # Handle Ctrl+C gracefully
     def signal_handler(sig, frame):
         print("\nShutting down...")
-            executor.shutdown()
+        sys.exit(0)
     
     signal.signal(signal.SIGINT, signal_handler)
     
-        executor.spin()
+    # Run the launch command
-        
+    try:
-    except Exception as e:
+        subprocess.run(launch_cmd, check=True)
-        print(f"Error: {e}")
+    except subprocess.CalledProcessError as e:
-        raise
+        print(f"[ERROR] Launch failed: {e}")
-    finally:
+        return 1
-        print("Cleaning up...")
+    except KeyboardInterrupt:
-        
+        print("\nShutdown complete.")
-        # Stop nodes
-        for node in nodes:
-            node.destroy_node()
-        
-        # Stop ROS
-        if rclpy.ok():
-            rclpy.shutdown()
-        
-        # Stop SITL
-        if sitl:
-            sitl.stop()
-        
-        print("Shutdown complete.")
     
     return 0
 

setup/install_ardupilot.sh

@@ -0,0 +1,351 @@
+#!/bin/bash
+# =============================================================================
+# ArduPilot ROS 2 + Gazebo Installation Script
+# =============================================================================
+# Installs the official ArduPilot ROS 2 packages with DDS and Gazebo support.
+# This is the recommended way to run ArduPilot SITL with ROS 2.
+#
+# Prerequisites:
+#   - Ubuntu 22.04 (ROS 2 Humble)
+#   - ROS 2 Humble installed
+#
+# Usage: ./install_ardupilot.sh [--skip-sitl] [--skip-gazebo]
+#
+# This installs:
+#   1. ArduPilot SITL with DDS support
+#   2. ardupilot_ros - ArduPilot ROS 2 packages
+#   3. ardupilot_gz - ArduPilot Gazebo integration
+#   4. MAVProxy ground control station
+# =============================================================================
+
+set -e
+
+echo "=============================================="
+echo "  ArduPilot ROS 2 + Gazebo Installation"
+echo "=============================================="
+echo ""
+
+# Parse arguments
+SKIP_SITL=false
+SKIP_GAZEBO=false
+for arg in "$@"; do
+    case $arg in
+        --skip-sitl)
+            SKIP_SITL=true
+            ;;
+        --skip-gazebo)
+            SKIP_GAZEBO=true
+            ;;
+    esac
+done
+
+# Get script directory
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+PROJECT_ROOT="$(dirname "$SCRIPT_DIR")"
+
+# Directories
+ARDUPILOT_WS="$HOME/ardu_ws"
+ARDUPILOT_DIR="$HOME/ardupilot"
+
+# Check for ROS 2
+if [ ! -f "/opt/ros/humble/setup.bash" ] && [ ! -f "/opt/ros/jazzy/setup.bash" ]; then
+    echo "[ERROR] ROS 2 not found!"
+    echo "Please install ROS 2 first:"
+    echo "  ./setup/install_ubuntu.sh"
+    exit 1
+fi
+
+# Source ROS 2
+if [ -f "/opt/ros/humble/setup.bash" ]; then
+    source /opt/ros/humble/setup.bash
+    ROS_DISTRO="humble"
+    GZ_VERSION="harmonic"
+elif [ -f "/opt/ros/jazzy/setup.bash" ]; then
+    source /opt/ros/jazzy/setup.bash
+    ROS_DISTRO="jazzy"
+    GZ_VERSION="harmonic"
+fi
+
+echo "[INFO] ROS 2 Distro: $ROS_DISTRO"
+echo "[INFO] Gazebo Version: $GZ_VERSION"
+echo "[INFO] Workspace: $ARDUPILOT_WS"
+echo ""
+
+# -----------------------------------------------------------------------------
+# Step 1: Install System Dependencies
+# -----------------------------------------------------------------------------
+echo "[STEP 1/7] Installing system dependencies..."
+
+sudo apt-get update
+sudo apt-get install -y \
+    git \
+    cmake \
+    build-essential \
+    python3 \
+    python3-pip \
+    python3-dev \
+    python3-venv \
+    python3-vcstool \
+    python3-rosdep \
+    python3-colcon-common-extensions \
+    wget \
+    curl
+
+# Install additional dependencies
+sudo apt-get install -y \
+    default-jre \
+    libxml2-dev \
+    libxslt1-dev \
+    libtool \
+    automake \
+    autoconf \
+    libexpat1-dev \
+    ccache || true
+
+echo "[INFO] System dependencies installed"
+
+# -----------------------------------------------------------------------------
+# Step 2: Install Gazebo Harmonic
+# -----------------------------------------------------------------------------
+if [ "$SKIP_GAZEBO" = false ]; then
+    echo ""
+    echo "[STEP 2/7] Installing Gazebo $GZ_VERSION..."
+    
+    # Add Gazebo APT sources
+    sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
+    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
+    sudo apt-get update
+    
+    # Install Gazebo
+    sudo apt-get install -y gz-$GZ_VERSION || {
+        echo "[WARN] Could not install gz-$GZ_VERSION, trying gz-garden..."
+        sudo apt-get install -y gz-garden || true
+    }
+    
+    echo "[INFO] Gazebo installed"
+else
+    echo ""
+    echo "[STEP 2/7] Skipping Gazebo installation (--skip-gazebo)"
+fi
+
+# -----------------------------------------------------------------------------
+# Step 3: Create ArduPilot ROS 2 Workspace
+# -----------------------------------------------------------------------------
+echo ""
+echo "[STEP 3/7] Setting up ArduPilot ROS 2 workspace..."
+
+mkdir -p "$ARDUPILOT_WS/src"
+cd "$ARDUPILOT_WS"
+
+# Import ArduPilot ROS 2 repositories using vcstool
+echo "[INFO] Importing ArduPilot ROS 2 repositories..."
+
+# Create ros2.repos file for ArduPilot packages
+cat > /tmp/ardupilot_ros2.repos << 'EOF'
+repositories:
+  ardupilot:
+    type: git
+    url: https://github.com/ArduPilot/ardupilot.git
+    version: master
+  ardupilot_gz:
+    type: git
+    url: https://github.com/ArduPilot/ardupilot_gz.git
+    version: main
+EOF
+
+vcs import --recursive src < /tmp/ardupilot_ros2.repos
+
+# Also import Gazebo-related packages
+if [ "$SKIP_GAZEBO" = false ]; then
+    echo "[INFO] Importing Gazebo packages..."
+    vcs import --input https://raw.githubusercontent.com/ArduPilot/ardupilot_gz/main/ros2_gz.repos --recursive src || true
+fi
+
+echo "[INFO] ArduPilot ROS 2 repositories imported"
+
+# -----------------------------------------------------------------------------
+# Step 4: Install ArduPilot SITL Prerequisites
+# -----------------------------------------------------------------------------
+if [ "$SKIP_SITL" = false ]; then
+    echo ""
+    echo "[STEP 4/7] Installing ArduPilot SITL prerequisites..."
+    
+    cd "$ARDUPILOT_WS/src/ardupilot"
+    
+    # Run ArduPilot prerequisites installer
+    Tools/environment_install/install-prereqs-ubuntu.sh -y
+    
+    # Reload profile
+    . ~/.profile || true
+    
+    echo "[INFO] ArduPilot SITL prerequisites installed"
+else
+    echo ""
+    echo "[STEP 4/7] Skipping SITL prerequisites (--skip-sitl)"
+fi
+
+# -----------------------------------------------------------------------------
+# Step 5: Configure rosdep for Gazebo
+# -----------------------------------------------------------------------------
+echo ""
+echo "[STEP 5/7] Configuring rosdep..."
+
+# Initialize rosdep if needed
+if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then
+    sudo rosdep init || true
+fi
+rosdep update
+
+# Add Gazebo sources to rosdep (for non-default ROS 2 Humble + Gazebo Harmonic pairing)
+if [ "$ROS_DISTRO" = "humble" ] && [ "$GZ_VERSION" = "harmonic" ]; then
+    sudo wget https://raw.githubusercontent.com/osrf/osrf-rosdep/master/gz/00-gazebo.list \
+        -O /etc/ros/rosdep/sources.list.d/00-gazebo.list || true
+    rosdep update || true
+fi
+
+# Install ROS dependencies
+cd "$ARDUPILOT_WS"
+rosdep install --from-paths src --ignore-src -y || true
+
+echo "[INFO] rosdep configured"
+
+# -----------------------------------------------------------------------------
+# Step 6: Build ArduPilot ROS 2 Packages
+# -----------------------------------------------------------------------------
+echo ""
+echo "[STEP 6/7] Building ArduPilot ROS 2 packages..."
+
+cd "$ARDUPILOT_WS"
+source /opt/ros/$ROS_DISTRO/setup.bash
+
+# Set Gazebo version environment variable
+export GZ_VERSION=$GZ_VERSION
+
+# Build packages
+if [ "$SKIP_GAZEBO" = false ]; then
+    echo "[INFO] Building with Gazebo support..."
+    colcon build --packages-up-to ardupilot_gz_bringup --symlink-install || {
+        echo "[WARN] Full build failed, trying core packages only..."
+        colcon build --packages-up-to ardupilot_sitl --symlink-install
+    }
+else
+    echo "[INFO] Building SITL only..."
+    colcon build --packages-up-to ardupilot_sitl --symlink-install
+fi
+
+echo "[INFO] ArduPilot ROS 2 packages built"
+
+# -----------------------------------------------------------------------------
+# Step 7: Configure Environment Variables
+# -----------------------------------------------------------------------------
+echo ""
+echo "[STEP 7/7] Configuring environment variables..."
+
+BASHRC_MARKER="# === ArduPilot ROS 2 Configuration ==="
+
+if ! grep -q "$BASHRC_MARKER" ~/.bashrc; then
+    echo "" >> ~/.bashrc
+    echo "$BASHRC_MARKER" >> ~/.bashrc
+    echo "" >> ~/.bashrc
+    
+    # Gazebo version
+    echo "# Gazebo Version" >> ~/.bashrc
+    echo "export GZ_VERSION=$GZ_VERSION" >> ~/.bashrc
+    echo "" >> ~/.bashrc
+    
+    # ArduPilot workspace
+    echo "# ArduPilot ROS 2 Workspace" >> ~/.bashrc
+    echo "export ARDUPILOT_WS=$ARDUPILOT_WS" >> ~/.bashrc
+    echo "" >> ~/.bashrc
+    
+    # ArduPilot SITL paths
+    echo "# ArduPilot SITL" >> ~/.bashrc
+    echo "export PATH=\$PATH:$ARDUPILOT_WS/src/ardupilot/Tools/autotest" >> ~/.bashrc
+    echo "" >> ~/.bashrc
+    
+    # Source ROS 2 and workspace
+    echo "# Source ROS 2 and ArduPilot workspace" >> ~/.bashrc
+    echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
+    echo "if [ -f $ARDUPILOT_WS/install/setup.bash ]; then" >> ~/.bashrc
+    echo "    source $ARDUPILOT_WS/install/setup.bash" >> ~/.bashrc
+    echo "fi" >> ~/.bashrc
+    echo "" >> ~/.bashrc
+    
+    echo "[INFO] Environment variables added to ~/.bashrc"
+else
+    echo "[INFO] Environment variables already configured in ~/.bashrc"
+fi
+
+# Install pymavlink in venv if it exists
+if [ -d "$PROJECT_ROOT/venv" ]; then
+    echo "[INFO] Installing pymavlink in project venv..."
+    source "$PROJECT_ROOT/venv/bin/activate"
+    pip install pymavlink mavproxy || true
+    deactivate
+fi
+
+# Also install pymavlink and mavproxy globally
+pip3 install --user pymavlink mavproxy || true
+
+# -----------------------------------------------------------------------------
+# Verification
+# -----------------------------------------------------------------------------
+echo ""
+echo "=============================================="
+echo "  Verifying Installation"
+echo "=============================================="
+echo ""
+
+# Source the workspace
+source "$ARDUPILOT_WS/install/setup.bash" || true
+
+# Check ROS 2 packages
+ros2 pkg list | grep -q "ardupilot_sitl" && echo "[OK] ardupilot_sitl package" || echo "[WARN] ardupilot_sitl not found"
+
+if [ "$SKIP_GAZEBO" = false ]; then
+    ros2 pkg list | grep -q "ardupilot_gz_bringup" && echo "[OK] ardupilot_gz_bringup package" || echo "[WARN] ardupilot_gz_bringup not found"
+fi
+
+# Check Gazebo
+if command -v gz &> /dev/null; then
+    echo "[OK] Gazebo (gz command)"
+elif command -v ign &> /dev/null; then
+    echo "[OK] Gazebo Fortress (ign command)"
+else
+    echo "[WARN] Gazebo command not found"
+fi
+
+# Check MAVProxy
+python3 -c "from pymavlink import mavutil" &> /dev/null && echo "[OK] pymavlink" || echo "[WARN] pymavlink not found"
+
+# -----------------------------------------------------------------------------
+# Complete
+# -----------------------------------------------------------------------------
+echo ""
+echo "=============================================="
+echo "  ArduPilot ROS 2 Installation Complete!"
+echo "=============================================="
+echo ""
+echo "IMPORTANT: Run the following to apply changes:"
+echo "  source ~/.bashrc"
+echo ""
+echo "Quick Start - Run SITL with ROS 2:"
+echo "  source ~/ardu_ws/install/setup.bash"
+echo "  ros2 launch ardupilot_sitl sitl_dds_udp.launch.py \\"
+echo "      transport:=udp4 \\"
+echo "      synthetic_clock:=True \\"
+echo "      model:=quad"
+echo ""
+echo "Quick Start - Run with Gazebo:"
+echo "  source ~/ardu_ws/install/setup.bash"
+echo "  ros2 launch ardupilot_gz_bringup iris_runway.launch.py"
+echo ""
+echo "MAVProxy (in another terminal):"
+echo "  mavproxy.py --console --map --master=:14550"
+echo ""
+echo "ROS 2 Topics:"
+echo "  ros2 topic list"
+echo "  ros2 topic echo /ap/geopose/filtered"
+echo ""
+echo "For more info, see: docs/ardupilot.md"
+echo ""

setup/install_ubuntu.sh

@@ -3,9 +3,11 @@
 # Drone Simulation - Ubuntu/Debian Installation Script
 # =============================================================================
 # Installs ROS 2, Gazebo, PyBullet, and all required dependencies
-# Includes optional ArduPilot SITL setup for realistic flight controller
+# Use --with-ardupilot to also install ArduPilot SITL
 #
-# Usage: ./install_ubuntu.sh [--with-ardupilot]
+# Usage: 
+#   ./install_ubuntu.sh                 # Basic installation
+#   ./install_ubuntu.sh --with-ardupilot  # Include ArduPilot SITL
 # =============================================================================
 
 set -e
@@ -242,62 +244,20 @@ python3 -c "from PIL import Image; print('  Pillow: OK')" || echo "  Pillow: FAI
 python3 -c "from pymavlink import mavutil; print('  pymavlink: OK')" || echo "  pymavlink: FAILED"
 
 # -----------------------------------------------------------------------------
-# Step 9: Optional ArduPilot SITL Installation
+# Step 9: ArduPilot SITL Installation (if requested)
 # -----------------------------------------------------------------------------
 if [ "$INSTALL_ARDUPILOT" = true ]; then
     echo ""
     echo "[STEP 9] Installing ArduPilot SITL..."
+    echo "[INFO] Calling dedicated ArduPilot install script..."
     
-    # Install ArduPilot dependencies
+    # Call the dedicated ArduPilot install script
-    sudo apt-get install -y \
+    if [ -f "$SCRIPT_DIR/install_ardupilot.sh" ]; then
-        python3-dev \
+        bash "$SCRIPT_DIR/install_ardupilot.sh"
-        python3-opencv \
-        python3-wxgtk4.0 \
-        python3-matplotlib \
-        python3-lxml \
-        libxml2-dev \
-        libxslt1-dev || true
-    
-    # Clone ArduPilot if not exists
-    if [ ! -d "$HOME/ardupilot" ]; then
-        echo "[INFO] Cloning ArduPilot..."
-        git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git "$HOME/ardupilot"
-        cd "$HOME/ardupilot"
-        Tools/environment_install/install-prereqs-ubuntu.sh -y
-        cd "$PROJECT_ROOT"
     else
-        echo "[INFO] ArduPilot already exists at $HOME/ardupilot"
+        echo "[ERROR] install_ardupilot.sh not found!"
+        echo "[INFO] Please run: ./setup/install_ardupilot.sh"
     fi
-    
-    # Clone ArduPilot Gazebo plugin if not exists
-    if [ ! -d "$HOME/ardupilot_gazebo" ]; then
-        echo "[INFO] Cloning ArduPilot Gazebo plugin..."
-        git clone https://github.com/ArduPilot/ardupilot_gazebo.git "$HOME/ardupilot_gazebo"
-        cd "$HOME/ardupilot_gazebo"
-        mkdir -p build && cd build
-        cmake .. -DCMAKE_BUILD_TYPE=Release
-        make -j$(nproc)
-        cd "$PROJECT_ROOT"
-    else
-        echo "[INFO] ArduPilot Gazebo plugin already exists at $HOME/ardupilot_gazebo"
-    fi
-    
-    # Add to bashrc
-    if ! grep -q "ARDUPILOT_HOME" ~/.bashrc; then
-        echo '' >> ~/.bashrc
-        echo '# ArduPilot SITL' >> ~/.bashrc
-        echo 'export ARDUPILOT_HOME=$HOME/ardupilot' >> ~/.bashrc
-        echo 'export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest' >> ~/.bashrc
-    fi
-    
-    if ! grep -q "ardupilot_gazebo" ~/.bashrc; then
-        echo '' >> ~/.bashrc
-        echo '# ArduPilot Gazebo Plugin' >> ~/.bashrc
-        echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH' >> ~/.bashrc
-        echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH' >> ~/.bashrc
-    fi
-    
-    echo "[INFO] ArduPilot SITL installed"
 fi
 
 echo ""