Added Docker Files

.dockerignore

@@ -0,0 +1,61 @@
+# =============================================================================
+# Docker Build Ignore
+# =============================================================================
+# Files/directories to exclude from the Docker build context.
+# This speeds up builds and keeps sensitive files out of the image.
+# =============================================================================
+
+# Git
+.git
+.gitignore
+
+# Python
+__pycache__
+*.py[cod]
+*$py.class
+*.so
+.Python
+venv/
+ENV/
+env/
+.venv/
+
+# IDE
+.vscode/
+.idea/
+*.swp
+*.swo
+*~
+
+# Build artifacts
+*.egg-info/
+dist/
+build/
+*.egg
+
+# Docker
+Dockerfile*
+docker-compose*.yml
+.docker/
+
+# Logs
+*.log
+logs/
+
+# Temp files
+*.tmp
+*.temp
+.cache/
+
+# OS files
+.DS_Store
+Thumbs.db
+
+# Simulation outputs
+recordings/
+*.webp
+*.mp4
+*.avi
+
+# Documentation (optional - include if needed)
+# docs/

Dockerfile

@@ -0,0 +1,171 @@
+# =============================================================================
+# RDC Simulation - Docker Build (GPU-enabled)
+# =============================================================================
+# Builds a complete ArduPilot + Gazebo simulation environment with GPU support.
+# Your host system bashrc is NOT modified - everything stays in the container.
+#
+# Build:
+#   docker build -t rdc-simulation .
+#
+# Run (with GPU):
+#   docker run --gpus all -it --rm \
+#     -e DISPLAY=$DISPLAY \
+#     -v /tmp/.X11-unix:/tmp/.X11-unix \
+#     rdc-simulation
+#
+# Or use docker-compose:
+#   docker compose up
+# =============================================================================
+
+FROM ubuntu:24.04
+
+# Avoid interactive prompts during build
+ENV DEBIAN_FRONTEND=noninteractive
+ENV TZ=UTC
+
+# =============================================================================
+# SYSTEM DEPENDENCIES
+# =============================================================================
+RUN apt-get update && apt-get install -y \
+    # Build tools
+    curl \
+    wget \
+    git \
+    cmake \
+    build-essential \
+    gnupg \
+    lsb-release \
+    software-properties-common \
+    # Python
+    python3 \
+    python3-pip \
+    python3-venv \
+    python3-dev \
+    # Graphics / X11
+    libgl1-mesa-glx \
+    libgl1-mesa-dri \
+    mesa-utils \
+    x11-apps \
+    # Video recording
+    ffmpeg \
+    xdotool \
+    wmctrl \
+    # Networking
+    netcat-openbsd \
+    # GStreamer (for ArduPilot Gazebo plugin)
+    libgstreamer1.0-dev \
+    libgstreamer-plugins-base1.0-dev \
+    gstreamer1.0-plugins-bad \
+    gstreamer1.0-libav \
+    gstreamer1.0-gl \
+    # OpenCV
+    libopencv-dev \
+    && rm -rf /var/lib/apt/lists/*
+
+# =============================================================================
+# ROS 2 JAZZY
+# =============================================================================
+RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \
+    && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu noble main" > /etc/apt/sources.list.d/ros2.list \
+    && apt-get update \
+    && apt-get install -y \
+        ros-jazzy-ros-base \
+        ros-jazzy-geometry-msgs \
+        ros-jazzy-std-msgs \
+        ros-jazzy-nav-msgs \
+        ros-jazzy-sensor-msgs \
+        ros-jazzy-ros-gz \
+    && rm -rf /var/lib/apt/lists/*
+
+# =============================================================================
+# GAZEBO HARMONIC
+# =============================================================================
+RUN 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 noble main" > /etc/apt/sources.list.d/gazebo-stable.list \
+    && apt-get update \
+    && apt-get install -y gz-harmonic \
+    && rm -rf /var/lib/apt/lists/*
+
+# =============================================================================
+# CREATE NON-ROOT USER
+# =============================================================================
+ARG USERNAME=pilot
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN groupadd --gid $USER_GID $USERNAME \
+    && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
+    && apt-get update \
+    && apt-get install -y sudo \
+    && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
+    && chmod 0440 /etc/sudoers.d/$USERNAME \
+    && rm -rf /var/lib/apt/lists/*
+
+USER $USERNAME
+WORKDIR /home/$USERNAME
+
+# =============================================================================
+# ARDUPILOT SITL
+# =============================================================================
+RUN git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git /home/$USERNAME/ardupilot
+
+# Install ArduPilot prerequisites (writes to container's bashrc, not host)
+WORKDIR /home/$USERNAME/ardupilot
+RUN Tools/environment_install/install-prereqs-ubuntu.sh -y
+
+# Build ArduCopter SITL
+RUN . ~/.profile && ./waf configure --board sitl && ./waf copter
+
+# =============================================================================
+# ARDUPILOT GAZEBO PLUGIN
+# =============================================================================
+RUN git clone https://github.com/ArduPilot/ardupilot_gazebo.git /home/$USERNAME/ardupilot_gazebo
+
+WORKDIR /home/$USERNAME/ardupilot_gazebo
+RUN mkdir -p build && cd build \
+    && cmake .. -DCMAKE_BUILD_TYPE=Release \
+    && make -j$(nproc)
+
+# =============================================================================
+# PYTHON DEPENDENCIES (MAVProxy, pymavlink)
+# =============================================================================
+RUN pip3 install --user --break-system-packages pymavlink mavproxy pexpect pybullet numpy pillow opencv-python
+
+# =============================================================================
+# COPY PROJECT FILES
+# =============================================================================
+WORKDIR /home/$USERNAME/RDC_Simulation
+COPY --chown=$USERNAME:$USERNAME . .
+
+# Create venv and install requirements
+RUN python3 -m venv venv \
+    && . venv/bin/activate \
+    && pip install --upgrade pip \
+    && pip install -r requirements.txt
+
+# =============================================================================
+# ENVIRONMENT SETUP (container only - doesn't touch host)
+# =============================================================================
+ENV ARDUPILOT_HOME=/home/pilot/ardupilot
+ENV PATH="${PATH}:/home/pilot/ardupilot/Tools/autotest:/home/pilot/.local/bin"
+ENV GZ_SIM_SYSTEM_PLUGIN_PATH=/home/pilot/ardupilot_gazebo/build
+ENV GZ_SIM_RESOURCE_PATH=/home/pilot/ardupilot_gazebo/models:/home/pilot/ardupilot_gazebo/worlds:/home/pilot/RDC_Simulation/gazebo/models
+
+# Source ROS2 and ArduPilot env in bashrc (container's bashrc, not host)
+RUN echo 'source /opt/ros/jazzy/setup.bash' >> ~/.bashrc \
+    && echo 'source ~/.ardupilot_env 2>/dev/null || true' >> ~/.bashrc \
+    && echo 'source ~/RDC_Simulation/venv/bin/activate' >> ~/.bashrc \
+    && echo 'export ARDUPILOT_HOME=~/ardupilot' >> ~/.bashrc \
+    && echo 'export PATH=$PATH:~/ardupilot/Tools/autotest:~/.local/bin' >> ~/.bashrc \
+    && echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=~/ardupilot_gazebo/build' >> ~/.bashrc \
+    && echo 'export GZ_SIM_RESOURCE_PATH=~/ardupilot_gazebo/models:~/ardupilot_gazebo/worlds:~/RDC_Simulation/gazebo/models' >> ~/.bashrc
+
+# =============================================================================
+# ENTRYPOINT
+# =============================================================================
+COPY --chown=$USERNAME:$USERNAME docker-entrypoint.sh /home/$USERNAME/docker-entrypoint.sh
+RUN chmod +x /home/$USERNAME/docker-entrypoint.sh
+
+WORKDIR /home/$USERNAME/RDC_Simulation
+ENTRYPOINT ["/home/pilot/docker-entrypoint.sh"]
+CMD ["bash"]

docker-compose.yml

@@ -0,0 +1,82 @@
+# =============================================================================
+# RDC Simulation - Docker Compose (GPU-enabled)
+# =============================================================================
+# Runs the simulation with GPU acceleration and X11 display forwarding.
+#
+# Usage:
+#   # Build the container
+#   docker compose build
+#
+#   # Run interactive shell
+#   docker compose run --rm simulation
+#
+#   # Run specific command
+#   docker compose run --rm simulation ./scripts/run_ardupilot_sim.sh runway
+# =============================================================================
+
+services:
+  simulation:
+    build:
+      context: .
+      dockerfile: Dockerfile
+    image: rdc-simulation:latest
+    container_name: rdc-sim
+    
+    # GPU support (NVIDIA)
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: all
+              capabilities: [gpu]
+    
+    # Display forwarding for Gazebo GUI
+    environment:
+      - DISPLAY=${DISPLAY:-:0}
+      - NVIDIA_VISIBLE_DEVICES=all
+      - NVIDIA_DRIVER_CAPABILITIES=all
+      - QT_X11_NO_MITSHM=1
+    
+    # Mount X11 socket for display
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix:rw
+      - ~/.Xauthority:/home/pilot/.Xauthority:ro
+    
+    # Network mode for SITL communication
+    network_mode: host
+    
+    # Keep stdin open for interactive use
+    stdin_open: true
+    tty: true
+    
+    # Run as current user (optional - for file permission compatibility)
+    # user: "${UID:-1000}:${GID:-1000}"
+    
+    working_dir: /home/pilot/RDC_Simulation
+
+  # Alternative: Run headless (no display, for CI/testing)
+  simulation-headless:
+    build:
+      context: .
+      dockerfile: Dockerfile
+    image: rdc-simulation:latest
+    container_name: rdc-sim-headless
+    
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: all
+              capabilities: [gpu]
+    
+    environment:
+      - NVIDIA_VISIBLE_DEVICES=all
+      - NVIDIA_DRIVER_CAPABILITIES=compute,utility
+      - HEADLESS=1
+    
+    network_mode: host
+    stdin_open: true
+    tty: true
+    working_dir: /home/pilot/RDC_Simulation

docker-entrypoint.sh

@@ -0,0 +1,54 @@
+#!/bin/bash
+# =============================================================================
+# Docker Entrypoint - RDC Simulation
+# =============================================================================
+# Sets up the environment inside the container before running commands.
+# =============================================================================
+
+set -e
+
+# Source ROS 2
+if [ -f "/opt/ros/jazzy/setup.bash" ]; then
+    source /opt/ros/jazzy/setup.bash
+fi
+
+# Source ArduPilot environment
+if [ -f "$HOME/.ardupilot_env" ]; then
+    source "$HOME/.ardupilot_env"
+fi
+
+# Activate Python venv
+if [ -f "$HOME/RDC_Simulation/venv/bin/activate" ]; then
+    source "$HOME/RDC_Simulation/venv/bin/activate"
+fi
+
+# Set up paths
+export ARDUPILOT_HOME=$HOME/ardupilot
+export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest:$HOME/.local/bin
+export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build
+export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$HOME/RDC_Simulation/gazebo/models
+
+# If no command provided, print help
+if [ "$1" = "bash" ] || [ -z "$1" ]; then
+    echo "=============================================="
+    echo "  RDC Simulation Container"
+    echo "=============================================="
+    echo ""
+    echo "Environment ready! Commands:"
+    echo ""
+    echo "  # Start Gazebo (Terminal 1)"
+    echo "  ./scripts/run_ardupilot_sim.sh runway"
+    echo ""
+    echo "  # Start SITL (Terminal 2)"  
+    echo "  sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console"
+    echo ""
+    echo "  # Run controller (Terminal 3)"
+    echo "  python scripts/run_ardupilot.py --pattern square"
+    echo ""
+    echo "Or run all in one:"
+    echo "  ./scripts/run_ardupilot_controller.sh"
+    echo ""
+    exec /bin/bash
+else
+    exec "$@"
+fi

scripts/record_flight.py

@@ -0,0 +1,240 @@
+#!/usr/bin/env python3
+"""
+Record Drone Simulation with Flight
+====================================
+Runs the drone controller while recording the Gazebo simulation to video.
+
+This script:
+1. Starts screen recording (ffmpeg)
+2. Runs the drone flight pattern
+3. Stops recording and saves the video
+
+Usage:
+    python scripts/record_flight.py --pattern square --duration 120
+    python scripts/record_flight.py --pattern circle --output my_flight
+"""
+
+import os
+import sys
+import time
+import signal
+import argparse
+import subprocess
+import threading
+from datetime import datetime
+
+# Add parent directory to path
+sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from src.drone_controller import DroneController
+
+
+class SimulationRecorder:
+    """Records the simulation display using ffmpeg."""
+    
+    def __init__(self, output_dir="recordings", fps=30, quality="medium"):
+        self.output_dir = output_dir
+        self.fps = fps
+        self.quality = quality
+        self.process = None
+        self.recording = False
+        self.output_file = None
+        
+        # Quality presets (CRF values for libx264)
+        self.quality_presets = {
+            "low": (28, "faster"),
+            "medium": (23, "medium"),
+            "high": (18, "slow")
+        }
+        
+        os.makedirs(output_dir, exist_ok=True)
+    
+    def start(self, output_name=None):
+        """Start recording."""
+        if self.recording:
+            print("[WARN] Already recording")
+            return False
+        
+        if not output_name:
+            output_name = f"flight_{datetime.now().strftime('%Y%m%d_%H%M%S')}"
+        
+        self.output_file = os.path.join(self.output_dir, f"{output_name}.mp4")
+        
+        crf, preset = self.quality_presets.get(self.quality, (23, "medium"))
+        display = os.environ.get("DISPLAY", ":0")
+        
+        # FFmpeg command for screen recording
+        cmd = [
+            "ffmpeg", "-y",
+            "-f", "x11grab",
+            "-framerate", str(self.fps),
+            "-i", display,
+            "-c:v", "libx264",
+            "-preset", preset,
+            "-crf", str(crf),
+            "-pix_fmt", "yuv420p",
+            self.output_file
+        ]
+        
+        print(f"[INFO] Starting recording: {self.output_file}")
+        
+        try:
+            self.process = subprocess.Popen(
+                cmd,
+                stdin=subprocess.PIPE,
+                stdout=subprocess.DEVNULL,
+                stderr=subprocess.DEVNULL
+            )
+            self.recording = True
+            return True
+        except FileNotFoundError:
+            print("[ERROR] ffmpeg not found. Install with: sudo apt install ffmpeg")
+            return False
+        except Exception as e:
+            print(f"[ERROR] Failed to start recording: {e}")
+            return False
+    
+    def stop(self):
+        """Stop recording."""
+        if not self.recording or not self.process:
+            return None
+        
+        print("[INFO] Stopping recording...")
+        
+        # Send 'q' to ffmpeg to stop gracefully
+        try:
+            self.process.communicate(input=b'q', timeout=5)
+        except subprocess.TimeoutExpired:
+            self.process.kill()
+        
+        self.recording = False
+        
+        if os.path.exists(self.output_file):
+            size = os.path.getsize(self.output_file)
+            print(f"[OK] Recording saved: {self.output_file} ({size / 1024 / 1024:.1f} MB)")
+            return self.output_file
+        else:
+            print("[WARN] Recording file not found")
+            return None
+
+
+def run_recorded_flight(pattern="square", altitude=5.0, size=5.0, output_name=None, quality="medium"):
+    """Run a drone flight while recording."""
+    
+    print("=" * 50)
+    print("  Recorded Drone Flight")
+    print("=" * 50)
+    print(f"  Pattern: {pattern}")
+    print(f"  Altitude: {altitude}m")
+    print(f"  Size: {size}m")
+    print(f"  Recording Quality: {quality}")
+    print("=" * 50)
+    print()
+    
+    # Start recorder
+    recorder = SimulationRecorder(quality=quality)
+    
+    # Initialize controller
+    controller = DroneController()
+    
+    if not controller.connect():
+        print("[ERROR] Could not connect to SITL")
+        return False
+    
+    # Start recording
+    recorder.start(output_name)
+    time.sleep(2)  # Give recording time to start
+    
+    try:
+        # Setup for GPS-denied flight
+        print("\n--- SETUP ---")
+        controller.setup_gps_denied()
+        
+        if not controller.set_mode("GUIDED_NOGPS"):
+            raise Exception("Could not set GUIDED_NOGPS mode")
+        
+        if not controller.arm():
+            raise Exception("Could not arm")
+        
+        # Takeoff
+        print("\n--- TAKEOFF ---")
+        if not controller.takeoff(altitude):
+            print("[WARN] Takeoff may have failed, continuing anyway...")
+        
+        time.sleep(2)
+        
+        # Fly pattern
+        print("\n--- FLY PATTERN ---")
+        if pattern == "square":
+            controller.fly_square(size=size, altitude=altitude)
+        elif pattern == "circle":
+            controller.fly_circle(radius=size, altitude=altitude)
+        else:
+            # Hover
+            print("[INFO] Hovering...")
+            time.sleep(10)
+        
+        # Land
+        print("\n--- LANDING ---")
+        controller.land()
+        
+        # Wait for landing
+        print("[INFO] Waiting for landing...")
+        for i in range(100):
+            controller.update_state()
+            if controller.altitude < 0.5:
+                print(f"[OK] Landed at altitude {controller.altitude:.1f}m")
+                break
+            time.sleep(0.1)
+        
+        time.sleep(2)
+        
+    except KeyboardInterrupt:
+        print("\n[INFO] Interrupted - landing...")
+        controller.land()
+        time.sleep(3)
+    except Exception as e:
+        print(f"\n[ERROR] {e}")
+        controller.land()
+    finally:
+        # Stop recording
+        time.sleep(1)
+        video_file = recorder.stop()
+        
+        print()
+        print("=" * 50)
+        print("  Flight Complete")
+        print("=" * 50)
+        if video_file:
+            print(f"  Video: {video_file}")
+        print()
+    
+    return True
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Record Drone Flight Simulation")
+    parser.add_argument("--pattern", "-p", choices=["square", "circle", "hover"],
+                        default="square", help="Flight pattern")
+    parser.add_argument("--altitude", "-a", type=float, default=5.0,
+                        help="Flight altitude (meters)")
+    parser.add_argument("--size", "-s", type=float, default=5.0,
+                        help="Pattern size (meters)")
+    parser.add_argument("--output", "-o", type=str, default=None,
+                        help="Output video filename (without extension)")
+    parser.add_argument("--quality", "-q", choices=["low", "medium", "high"],
+                        default="medium", help="Video quality")
+    
+    args = parser.parse_args()
+    
+    run_recorded_flight(
+        pattern=args.pattern,
+        altitude=args.altitude,
+        size=args.size,
+        output_name=args.output,
+        quality=args.quality
+    )
+
+
+if __name__ == "__main__":
+    main()

scripts/record_simulation.sh

@@ -0,0 +1,235 @@
+#!/bin/bash
+# =============================================================================
+# Record Gazebo Simulation to Video
+# =============================================================================
+# Records the Gazebo simulation window to a video file.
+#
+# Usage:
+#   ./scripts/record_simulation.sh                    # Default: record for 60s
+#   ./scripts/record_simulation.sh --duration 120     # Record for 120 seconds
+#   ./scripts/record_simulation.sh --output my_video  # Custom output name
+#   ./scripts/record_simulation.sh --method gazebo    # Use Gazebo's recorder
+#
+# Prerequisites:
+#   - Gazebo must be running
+#   - For ffmpeg method: sudo apt install ffmpeg
+# =============================================================================
+
+set -e
+
+# Default values
+DURATION=60
+OUTPUT_NAME="drone_simulation_$(date +%Y%m%d_%H%M%S)"
+OUTPUT_DIR="./recordings"
+METHOD="ffmpeg"  # ffmpeg or gazebo
+FPS=30
+QUALITY="medium"  # low, medium, high
+
+# Parse arguments
+while [[ $# -gt 0 ]]; do
+    case $1 in
+        -d|--duration)
+            DURATION="$2"
+            shift 2
+            ;;
+        -o|--output)
+            OUTPUT_NAME="$2"
+            shift 2
+            ;;
+        --dir)
+            OUTPUT_DIR="$2"
+            shift 2
+            ;;
+        -m|--method)
+            METHOD="$2"
+            shift 2
+            ;;
+        --fps)
+            FPS="$2"
+            shift 2
+            ;;
+        -q|--quality)
+            QUALITY="$2"
+            shift 2
+            ;;
+        -h|--help)
+            echo "Usage: $0 [OPTIONS]"
+            echo ""
+            echo "Options:"
+            echo "  -d, --duration SEC   Recording duration in seconds (default: 60)"
+            echo "  -o, --output NAME    Output filename without extension"
+            echo "  --dir PATH           Output directory (default: ./recordings)"
+            echo "  -m, --method METHOD  Recording method: ffmpeg or gazebo"
+            echo "  --fps FPS            Frames per second (default: 30)"
+            echo "  -q, --quality QUAL   Quality: low, medium, high (default: medium)"
+            echo "  -h, --help           Show this help"
+            exit 0
+            ;;
+        *)
+            echo "Unknown option: $1"
+            exit 1
+            ;;
+    esac
+done
+
+# Create output directory
+mkdir -p "$OUTPUT_DIR"
+
+# Quality presets
+case $QUALITY in
+    low)
+        CRF=28
+        PRESET="faster"
+        ;;
+    medium)
+        CRF=23
+        PRESET="medium"
+        ;;
+    high)
+        CRF=18
+        PRESET="slow"
+        ;;
+esac
+
+echo "=============================================="
+echo "  Gazebo Simulation Recorder"
+echo "=============================================="
+echo "  Duration: ${DURATION}s"
+echo "  Output: ${OUTPUT_DIR}/${OUTPUT_NAME}.mp4"
+echo "  Method: $METHOD"
+echo "  FPS: $FPS"
+echo "  Quality: $QUALITY"
+echo "=============================================="
+echo ""
+
+if [ "$METHOD" = "gazebo" ]; then
+    # ==========================================================================
+    # METHOD 1: Gazebo's built-in video recording
+    # ==========================================================================
+    # Gazebo Harmonic can record video via the GUI or command line
+    
+    OUTPUT_FILE="${OUTPUT_DIR}/${OUTPUT_NAME}.mp4"
+    
+    echo "[INFO] Using Gazebo's built-in recorder..."
+    echo "[INFO] In Gazebo GUI:"
+    echo "       1. Click the hamburger menu (☰) in top-left"
+    echo "       2. Select 'Video Recorder'"
+    echo "       3. Click 'Record' to start, 'Stop' to stop"
+    echo "       4. Video saves to ~/.gz/sim/recordings/"
+    echo ""
+    echo "[INFO] Or use gz service to record programmatically..."
+    
+    # Check if Gazebo is running
+    if ! pgrep -x "gz" > /dev/null && ! pgrep -x "ruby" > /dev/null; then
+        echo "[ERROR] Gazebo doesn't appear to be running"
+        echo "[TIP] Start Gazebo first: ./scripts/run_ardupilot_sim.sh runway"
+        exit 1
+    fi
+    
+    # Start recording via gz service (if available)
+    echo "[INFO] Sending record command to Gazebo..."
+    gz service -s /gui/record_video --reqtype gz.msgs.VideoRecord --reptype gz.msgs.Boolean \
+        --timeout 5000 --req "start: true, format: \"mp4\", save_filename: \"${OUTPUT_FILE}\"" 2>/dev/null || {
+        echo "[WARN] Could not start recording via service"
+        echo "[TIP] Use the Gazebo GUI to record manually"
+    }
+    
+    echo "[INFO] Recording for ${DURATION} seconds..."
+    sleep "$DURATION"
+    
+    # Stop recording
+    gz service -s /gui/record_video --reqtype gz.msgs.VideoRecord --reptype gz.msgs.Boolean \
+        --timeout 5000 --req "start: false" 2>/dev/null || true
+    
+    echo "[OK] Recording stopped"
+    echo "[INFO] Check ~/.gz/sim/recordings/ for the video"
+
+else
+    # ==========================================================================
+    # METHOD 2: FFmpeg screen recording
+    # ==========================================================================
+    # Records the Gazebo window using ffmpeg
+    
+    OUTPUT_FILE="${OUTPUT_DIR}/${OUTPUT_NAME}.mp4"
+    
+    # Check for ffmpeg
+    if ! command -v ffmpeg &> /dev/null; then
+        echo "[ERROR] ffmpeg not found"
+        echo "[TIP] Install: sudo apt install ffmpeg"
+        exit 1
+    fi
+    
+    # Find the Gazebo window
+    echo "[INFO] Looking for Gazebo window..."
+    
+    # Try to find the window ID
+    WINDOW_ID=""
+    
+    # Try xdotool first
+    if command -v xdotool &> /dev/null; then
+        WINDOW_ID=$(xdotool search --name "Gazebo" 2>/dev/null | head -1) || true
+    fi
+    
+    # Fallback to wmctrl
+    if [ -z "$WINDOW_ID" ] && command -v wmctrl &> /dev/null; then
+        WINDOW_ID=$(wmctrl -l | grep -i "gazebo" | awk '{print $1}' | head -1) || true
+    fi
+    
+    if [ -n "$WINDOW_ID" ]; then
+        # Get window geometry
+        if command -v xdotool &> /dev/null; then
+            eval $(xdotool getwindowgeometry --shell "$WINDOW_ID" 2>/dev/null) || true
+        fi
+        
+        if [ -n "$WIDTH" ] && [ -n "$HEIGHT" ] && [ -n "$X" ] && [ -n "$Y" ]; then
+            echo "[INFO] Found Gazebo window: ${WIDTH}x${HEIGHT} at +${X}+${Y}"
+            GRAB_REGION="-video_size ${WIDTH}x${HEIGHT} -grab_x $X -grab_y $Y"
+        else
+            echo "[WARN] Could not get window geometry, recording full screen"
+            GRAB_REGION=""
+        fi
+    else
+        echo "[WARN] Could not find Gazebo window, recording full screen"
+        GRAB_REGION=""
+    fi
+    
+    echo "[INFO] Starting recording..."
+    echo "[INFO] Press Ctrl+C to stop early"
+    echo ""
+    
+    # Record using ffmpeg
+    # Using x11grab to capture the screen
+    ffmpeg -y \
+        -f x11grab \
+        -framerate "$FPS" \
+        $GRAB_REGION \
+        -i "${DISPLAY:-:0}" \
+        -t "$DURATION" \
+        -c:v libx264 \
+        -preset "$PRESET" \
+        -crf "$CRF" \
+        -pix_fmt yuv420p \
+        "$OUTPUT_FILE" \
+        2>&1 | while IFS= read -r line; do
+            # Show progress
+            if [[ "$line" =~ time=([0-9:\.]+) ]]; then
+                echo -ne "\r[Recording] Time: ${BASH_REMATCH[1]} / ${DURATION}s"
+            fi
+        done
+    
+    echo ""
+    echo ""
+    echo "[OK] Recording complete!"
+    echo "[INFO] Output: $OUTPUT_FILE"
+    
+    # Get file info
+    if [ -f "$OUTPUT_FILE" ]; then
+        FILE_SIZE=$(du -h "$OUTPUT_FILE" | cut -f1)
+        echo "[INFO] Size: $FILE_SIZE"
+    fi
+fi
+
+echo ""
+echo "=============================================="
+echo "  Recording Complete"
+echo "=============================================="

src/drone_controller.py

@@ -390,9 +390,14 @@ class DroneController:
             cr * cp * sy - sr * sp * cy   # z
         ]
         
-        # type_mask: bit 7 = ignore body roll/pitch rate
+        # type_mask bits:
-        # We provide quaternion and thrust
+        # bit 0: ignore body roll rate
-        type_mask = 0b00000111  # Ignore body rates, use quaternion + thrust
+        # bit 1: ignore body pitch rate  
+        # bit 2: ignore body yaw rate
+        # bit 6: ignore thrust
+        # bit 7: attitude (quaternion) is valid
+        # We want: use quaternion (clear bit 7), use thrust (clear bit 6), ignore body rates (set bits 0-2)
+        type_mask = 0b00000111  # Use attitude + thrust, ignore body rates
         
         self.mav.mav.set_attitude_target_send(
             0,  # time_boot_ms
@@ -404,41 +409,79 @@ class DroneController:
             thrust          # Thrust 0-1
         )
     
-    def goto(self, x, y, z):
+    def _send_velocity_ned(self, vx, vy, vz):
-        """Go to position (NED frame, z is down so negative = up)."""
+        """Send velocity command in NED frame."""
-        print(f"[INFO] Going to ({x:.1f}, {y:.1f}, alt={-z:.1f}m)...")
+        # type_mask bits (1=ignore, 0=use):
+        # bits 0-2: position x,y,z
+        # bits 3-5: velocity x,y,z  
+        # bits 6-8: acceleration x,y,z
+        # bit 9: force
+        # bit 10: yaw
+        # bit 11: yaw_rate
+        # Standard velocity-only mask: ignore pos, use vel, ignore accel, ignore yaw
+        type_mask = (
+            (1 << 0) | (1 << 1) | (1 << 2) |  # ignore position
+            # velocity bits 3,4,5 are 0 (use)
+            (1 << 6) | (1 << 7) | (1 << 8) |  # ignore acceleration
+            (1 << 10) | (1 << 11)              # ignore yaw, yaw_rate
+        )  # = 0b110111000111 = 0xDC7
         
         self.mav.mav.set_position_target_local_ned_send(
             0,
             self.mav.target_system,
             self.mav.target_component,
             mavutil.mavlink.MAV_FRAME_LOCAL_NED,
-            0b0000111111111000,  # Position only
+            type_mask,
-            x, y, z,  # Position (NED)
+            0, 0, 0,      # Position (ignored)
-            0, 0, 0,  # Velocity
+            vx, vy, vz,   # Velocity NED
-            0, 0, 0,  # Acceleration
+            0, 0, 0,      # Acceleration (ignored)
             0, 0          # Yaw, yaw_rate
         )
     
-    def fly_to_and_wait(self, x, y, altitude, tolerance=0.5, timeout=30):
+    def fly_to_and_wait_absolute(self, target_x, target_y, altitude, tolerance=1.0, timeout=30):
-        """Fly to position and wait until reached."""
+        """Fly to absolute position using velocity commands."""
-        z = -altitude  # NED
+        
-        self.goto(x, y, z)
+        print(f"[INFO] Flying to ({target_x:.1f}, {target_y:.1f}, {altitude:.1f}m)")
         
         for i in range(int(timeout * 10)):
             self.update_state()
-            dx = x - self.position["x"]
-            dy = y - self.position["y"]
-            dist = math.sqrt(dx*dx + dy*dy)
             
-            if dist < tolerance:
+            dx = target_x - self.position["x"]
-                print(f"[OK] Reached waypoint ({x:.1f}, {y:.1f})")
+            dy = target_y - self.position["y"]
+            dz = altitude - self.altitude
+            
+            dist_horiz = math.sqrt(dx*dx + dy*dy)
+            
+            if dist_horiz < tolerance and abs(dz) < tolerance:
+                # Stop
+                self._send_velocity_ned(0, 0, 0)
+                print(f" [OK] Reached ({self.position['x']:.1f}, {self.position['y']:.1f})")
                 return True
             
+            # Calculate velocity towards target (in NED frame)
+            speed = min(2.0, max(0.5, dist_horiz * 0.5))  # 0.5-2.0 m/s
+            
+            if dist_horiz > 0.1:
+                vx = (dx / dist_horiz) * speed
+                vy = (dy / dist_horiz) * speed
+            else:
+                vx, vy = 0, 0
+            
+            # Vertical velocity
+            if dz > 0.5:
+                vz = -1.0  # Climb (negative = up in NED)
+            elif dz < -0.5:
+                vz = 0.5   # Descend
+            else:
+                vz = -dz   # Small adjustment
+            
+            self._send_velocity_ned(vx, vy, vz)
+            
             if i % 10 == 0:
-                print(f"\r    Distance: {dist:.1f}m", end="")
+                print(f"\r    Pos: ({self.position['x']:.1f}, {self.position['y']:.1f}) -> ({target_x:.1f}, {target_y:.1f}) Dist: {dist_horiz:.1f}m Alt: {self.altitude:.1f}m", end="")
             time.sleep(0.1)
         
+        self._send_velocity_ned(0, 0, 0)
         print(f"\n[WARN] Timeout reaching waypoint")
         return False
     
@@ -448,33 +491,46 @@ class DroneController:
         return self.set_mode("LAND")
     
     def fly_square(self, size=5, altitude=5):
-        """Fly a square pattern."""
+        """Fly a square pattern starting from current position."""
+        # Store initial position
+        self.update_state()
+        start_x = self.position["x"]
+        start_y = self.position["y"]
+        
+        # Waypoints are absolute positions based on start
         waypoints = [
-            (size, 0),
+            (start_x + size, start_y),         # Forward
-            (size, size),
+            (start_x + size, start_y + size),  # Right
-            (0, size),
+            (start_x, start_y + size),         # Back
-            (0, 0),
+            (start_x, start_y),                # Left (return)
         ]
         
         print(f"\n[INFO] Flying square pattern ({size}m x {size}m)")
+        print(f"[INFO] Start position: ({start_x:.1f}, {start_y:.1f})")
         
         for i, (x, y) in enumerate(waypoints):
-            print(f"\n--- Waypoint {i+1}/4 ---")
+            print(f"\n--- Waypoint {i+1}/4 ({x:.1f}, {y:.1f}) ---")
-            self.fly_to_and_wait(x, y, altitude)
+            self.fly_to_and_wait_absolute(x, y, altitude)
             time.sleep(1)
         
         print("\n[OK] Square pattern complete!")
     
     def fly_circle(self, radius=5, altitude=5, points=8):
-        """Fly a circular pattern."""
+        """Fly a circular pattern starting from current position."""
+        # Store initial position (center of circle)
+        self.update_state()
+        center_x = self.position["x"]
+        center_y = self.position["y"]
+        
         print(f"\n[INFO] Flying circle pattern (radius={radius}m)")
+        print(f"[INFO] Center: ({center_x:.1f}, {center_y:.1f})")
         
         for i in range(points + 1):
             angle = 2 * math.pi * i / points
-            x = radius * math.cos(angle)
+            x = center_x + radius * math.cos(angle)
-            y = radius * math.sin(angle)
+            y = center_y + radius * math.sin(angle)
-            print(f"\n--- Point {i+1}/{points+1} ---")
+            print(f"\n--- Point {i+1}/{points+1} ({x:.1f}, {y:.1f}) ---")
-            self.fly_to_and_wait(x, y, altitude)
+            self.fly_to_and_wait_absolute(x, y, altitude)
             time.sleep(0.5)
         
         print("\n[OK] Circle pattern complete!")