From 7a54455341cc51c4b55365629c9ef3f65e01eca8 Mon Sep 17 00:00:00 2001 From: default Date: Fri, 9 Jan 2026 20:51:14 +0000 Subject: [PATCH] Added Docker Files --- .dockerignore | 61 +++++++++ Dockerfile | 171 +++++++++++++++++++++++++ docker-compose.yml | 82 ++++++++++++ docker-entrypoint.sh | 54 ++++++++ scripts/record_flight.py | 240 +++++++++++++++++++++++++++++++++++ scripts/record_simulation.sh | 235 ++++++++++++++++++++++++++++++++++ src/drone_controller.py | 122 +++++++++++++----- 7 files changed, 932 insertions(+), 33 deletions(-) create mode 100644 .dockerignore create mode 100644 Dockerfile create mode 100644 docker-compose.yml create mode 100755 docker-entrypoint.sh create mode 100644 scripts/record_flight.py create mode 100755 scripts/record_simulation.sh diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..9fd659f --- /dev/null +++ b/.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/ diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..d447415 --- /dev/null +++ b/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"] diff --git a/docker-compose.yml b/docker-compose.yml new file mode 100644 index 0000000..06946d5 --- /dev/null +++ b/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 diff --git a/docker-entrypoint.sh b/docker-entrypoint.sh new file mode 100755 index 0000000..cc9def1 --- /dev/null +++ b/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 diff --git a/scripts/record_flight.py b/scripts/record_flight.py new file mode 100644 index 0000000..cf154f2 --- /dev/null +++ b/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() diff --git a/scripts/record_simulation.sh b/scripts/record_simulation.sh new file mode 100755 index 0000000..f066308 --- /dev/null +++ b/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 "==============================================" diff --git a/src/drone_controller.py b/src/drone_controller.py index 1c59ac4..07b5cdd 100644 --- a/src/drone_controller.py +++ b/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 - # We provide quaternion and thrust - type_mask = 0b00000111 # Ignore body rates, use quaternion + thrust + # type_mask bits: + # bit 0: ignore body roll rate + # 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): - """Go to position (NED frame, z is down so negative = up).""" - print(f"[INFO] Going to ({x:.1f}, {y:.1f}, alt={-z:.1f}m)...") + def _send_velocity_ned(self, vx, vy, vz): + """Send velocity command in NED frame.""" + # 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 - x, y, z, # Position (NED) - 0, 0, 0, # Velocity - 0, 0, 0, # Acceleration - 0, 0 # Yaw, yaw_rate + type_mask, + 0, 0, 0, # Position (ignored) + vx, vy, vz, # Velocity NED + 0, 0, 0, # Acceleration (ignored) + 0, 0 # Yaw, yaw_rate ) - def fly_to_and_wait(self, x, y, altitude, tolerance=0.5, timeout=30): - """Fly to position and wait until reached.""" - z = -altitude # NED - self.goto(x, y, z) + def fly_to_and_wait_absolute(self, target_x, target_y, altitude, tolerance=1.0, timeout=30): + """Fly to absolute position using velocity commands.""" + + 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: - print(f"[OK] Reached waypoint ({x:.1f}, {y:.1f})") + dx = target_x - self.position["x"] + 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), - (size, size), - (0, size), - (0, 0), + (start_x + size, start_y), # Forward + (start_x + size, start_y + size), # Right + (start_x, start_y + size), # Back + (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 ---") - self.fly_to_and_wait(x, y, altitude) + print(f"\n--- Waypoint {i+1}/4 ({x:.1f}, {y:.1f}) ---") + 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) - y = radius * math.sin(angle) - print(f"\n--- Point {i+1}/{points+1} ---") - self.fly_to_and_wait(x, y, altitude) + x = center_x + radius * math.cos(angle) + y = center_y + radius * math.sin(angle) + print(f"\n--- Point {i+1}/{points+1} ({x:.1f}, {y:.1f}) ---") + self.fly_to_and_wait_absolute(x, y, altitude) time.sleep(0.5) print("\n[OK] Circle pattern complete!")