Code reorganization. Display recording fixes. Search Flight Planner Fixes. Bug Fixes

README.md

@@ -18,6 +18,7 @@ A ROS 2 simulation environment for UAV and UGV development using GPS-denied navi
 - 16GB RAM recommended
 - 50GB disk space
 - NVIDIA GPU recommended (software rendering available)
+- `ffmpeg`, `x11-utils`, `xdotool` (for simulation recording)
 
 ## Installation
 

config/search.yaml

@@ -1,37 +1,23 @@
-# ─── Mission / Search Configuration ──────────────────────────
+altitude: 5.0
-# Single source of truth for all mission parameters.
-
-# ── Flight ───────────────────────────────────────────────────
-altitude: 5.0              # Search altitude (feet)
-
-# ── ArUco Marker ─────────────────────────────────────────────
 marker:
   dictionary: DICT_4X4_50
-  size: 0.5                # Physical marker size in meters
+  size: 0.5
-  landing_ids: [0]         # Marker IDs that trigger landing (on UGV)
+  landing_ids: [0]
-  target_ids: [1]          # Marker IDs to find and report to UGV
+  target_ids: [1]
-  target_position: [5.0, 3.0] # Initial X, Y location of the target Aruco map in the map
+  target_position: [5.0, 3.0]
-
-# ── Search Patterns ──────────────────────────────────────────
 spiral:
   max_legs: 35
-
 lawnmower:
   width: 30.0
   height: 30.0
-
 levy:
   max_steps: 40
   field_size: 50.0
-
-# ── Geofence ─────────────────────────────────────────────────
 geofence:
   enabled: true
   warning_distance: 3.0
   min_altitude: 0.0
   max_altitude: 15.0
-  # Polygon vertices in local NED (x=North, y=East) meters
-  # Centered on UAV start position (0, 0)
   points:
     - [-15, -15]
     - [-15, 15]

config/uav.yaml

@@ -1,11 +1,7 @@
-# ─── UAV Configuration ───────────────────────────────────────
-# UAV-specific settings. Mission config is in search.yaml.
-
 connection:
   sim: "tcp:127.0.0.1:5760"
   real: "/dev/ttyAMA0"
   baud: 57600
-
 speed:
   min_mph: 0.2
   max_mph: 1.0

config/ugv.yaml

@@ -1,17 +1,9 @@
-# ─── UGV Configuration ───────────────────────────────────────
-# UGV-specific settings. Mission config is in search.yaml.
-
-# UGV spawn position in local NED (meters)
-# The UAV starts on top of this — run_autonomous.sh syncs both
 position:
   x: 0.0
   y: 0.0
-
-# Gazebo transport topics
 topics:
   cmd_vel: /ugv/cmd_vel
   odometry: /ugv/odometry
-
 speed:
   min_mph: 0.5
   max_mph: 3.5

docs/setup_guide.md

@@ -6,6 +6,7 @@
 - 16GB RAM minimum
 - 50GB free disk space
 - Internet connection
+- `ffmpeg`, `x11-utils`, `xdotool` (installed automatically by `setup.sh`)
 
 ## Automatic Installation
 
@@ -33,11 +34,11 @@ bash setup.sh
 
 ## Manual Installation
 
-### Step 1: Install ROS 2
+### Step 1: Install ROS 2 and System Dependencies
 
 ```bash
 # Ubuntu 22.04
-sudo apt install software-properties-common
+sudo apt install software-properties-common x11-utils xdotool ffmpeg
 sudo add-apt-repository universe
 sudo apt update && sudo apt install curl -y
 sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg

docs/wsl_setup_guide.md

@@ -10,6 +10,7 @@ This guide covers running the UAV-UGV simulation on Windows Subsystem for Linux
 - WSL2 with Ubuntu 22.04
 - 16GB RAM minimum
 - Optional: NVIDIA GPU with WSL drivers
+- `ffmpeg`, `x11-utils`, `xdotool` (installed via `setup.sh` or `apt`)
 
 ## WSL2 Installation
 

scripts/generate_tags.py

@@ -50,7 +50,6 @@ DICT_SIZES = {
     "DICT_7X7_1000": 1000,
 }
 
-
 def generate_marker(dict_name, marker_id, pixel_size=600, border_bits=1):
     dict_id = ARUCO_DICTS[dict_name]
     aruco_dict = cv2.aruco.getPredefinedDictionary(dict_id)
@@ -65,7 +64,6 @@ def generate_marker(dict_name, marker_id, pixel_size=600, border_bits=1):
     padded[border_px:border_px + pixel_size, border_px:border_px + pixel_size] = marker_img
     return padded
 
-
 def get_landing_id():
     search_cfg = CONFIG_DIR / "search.yaml"
     if search_cfg.exists():
@@ -76,7 +74,6 @@ def get_landing_id():
             return land_ids[0]
     return 0
 
-
 def main():
     parser = argparse.ArgumentParser(description="Generate ArUco marker tags for Gazebo simulation")
     parser.add_argument("--dict", default="DICT_4X4_50", choices=list(ARUCO_DICTS.keys()))
@@ -135,6 +132,5 @@ def main():
 
     print(f"Done. {ok}/{len(ids)} markers verified.")
 
-
 if __name__ == "__main__":
     main()

scripts/generate_world.py

@@ -25,7 +25,7 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
 
     uav_x = ugv_x
     uav_y = ugv_y
-    uav_z = 0.40  # Start on top of UGV (0.18 top + 0.195 legs)
+    uav_z = 0.40
 
     marker = search_cfg.get("marker", {})
     target_pos = marker.get("target_position", [8.0, -6.0])
@@ -72,7 +72,7 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
         if geofence_cfg.get("enabled", False):
             points = geofence_cfg.get("points", [])
             if len(points) >= 3:
-                # Remove old geofence visual if it exists
+
                 for old_model in world.findall('model'):
                     if old_model.get('name') == 'geofence_visual':
                         world.remove(old_model)
@@ -100,7 +100,7 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
 
                     geometry = ET.SubElement(visual, "geometry")
                     box = ET.SubElement(geometry, "box")
-                    # size is Length(X), Width(Y), Thickness(Z)
+
                     ET.SubElement(box, "size").text = f"{length} 0.2 0.02"
 
                     material = ET.SubElement(visual, "material")

scripts/kill_simulation.sh

@@ -1,9 +1,7 @@
 #!/bin/bash
-
 echo "Killing all simulation processes..."
-
 pkill -9 -f "gz sim" 2>/dev/null || true
-pkill -9 -f "ruby" 2>/dev/null || true  # gz sim uses ruby
+pkill -9 -f "ruby" 2>/dev/null || true
 pkill -9 -f "gazebo" 2>/dev/null || true
 pkill -9 -f "gzserver" 2>/dev/null || true
 pkill -9 -f "gzclient" 2>/dev/null || true
@@ -15,9 +13,7 @@ pkill -9 -f "ArduRover" 2>/dev/null || true
 pkill -9 -f "arducopter" 2>/dev/null || true
 pkill -9 -f "autonomous_controller" 2>/dev/null || true
 pkill -9 -f "ros2" 2>/dev/null || true
-
 sleep 1
-
 echo "All processes killed."
 echo "Remaining ROS nodes:"
 ros2 node list 2>/dev/null || echo "No ROS nodes running"

scripts/run_autonomous.sh

@@ -1,25 +1,19 @@
 #!/bin/bash
-
 set -e
-
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
-
 RED='\033[0;31m'
 GREEN='\033[0;32m'
 YELLOW='\033[1;33m'
 CYAN='\033[0;36m'
 NC='\033[0m'
-
 print_info() { echo -e "${CYAN}[INFO]${NC} $1"; }
 print_success() { echo -e "${GREEN}[SUCCESS]${NC} $1"; }
 print_error() { echo -e "${RED}[ERROR]${NC} $1"; }
-
 SOFTWARE_RENDER=auto
 WORLD="uav_ugv_search.sdf"
 SEARCH="spiral"
 ALTITUDE=""
-
 while [[ $# -gt 0 ]]; do
     case $1 in
         --software-render) SOFTWARE_RENDER=true; shift ;;
@@ -39,7 +33,6 @@ while [[ $# -gt 0 ]]; do
         *) shift ;;
     esac
 done
-
 cleanup_all() {
     print_info "Cleaning up ..."
     pkill -f "camera_viewer.py" 2>/dev/null || true
@@ -50,7 +43,6 @@ cleanup_all() {
     pkill -f "mavproxy" 2>/dev/null || true
     pkill -f "main.py" 2>/dev/null || true
 }
-
 cleanup_sitl() {
     pkill -f "arducopter" 2>/dev/null || true
     pkill -f "sim_vehicle.py" 2>/dev/null || true
@@ -58,7 +50,6 @@ cleanup_sitl() {
     pkill -f "main.py" 2>/dev/null || true
 }
 trap cleanup_all EXIT
-
 if [ -f "$PROJECT_DIR/venv/bin/activate" ]; then
     print_info "Using venv: $PROJECT_DIR/venv"
     source "$PROJECT_DIR/venv/bin/activate"
@@ -66,18 +57,15 @@ elif [ -f "$PROJECT_DIR/.venv/bin/activate" ]; then
     print_info "Using .venv: $PROJECT_DIR/.venv"
     source "$PROJECT_DIR/.venv/bin/activate"
 fi
-
 export PATH=$PATH:$HOME/ardupilot/Tools/autotest:$HOME/.local/bin
 export GZ_SIM_RESOURCE_PATH="$PROJECT_DIR/models:$PROJECT_DIR/worlds:$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds"
 export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build
-
 print_info "Model path: $GZ_SIM_RESOURCE_PATH"
 if [ -f "$PROJECT_DIR/models/iris_with_gimbal/model.sdf" ]; then
     print_info "Using LOCAL iris_with_gimbal (with downward camera)"
 else
     print_info "Using ardupilot_gazebo iris_with_gimbal (NO downward camera)"
 fi
-
 if [ "$SOFTWARE_RENDER" = "auto" ]; then
     if grep -qi microsoft /proc/version 2>/dev/null; then
         print_info "WSL detected -> software rendering"
@@ -86,15 +74,12 @@ if [ "$SOFTWARE_RENDER" = "auto" ]; then
         SOFTWARE_RENDER=false
     fi
 fi
-
 if [ "$SOFTWARE_RENDER" = "true" ]; then
     export LIBGL_ALWAYS_SOFTWARE=1
     export GALLIUM_DRIVER=llvmpipe
     export MESA_GL_VERSION_OVERRIDE=3.3
 fi
-
 cleanup_all 2>/dev/null
-
 WORLD_FILE=""
 if [ -f "$PROJECT_DIR/worlds/$WORLD" ]; then
     WORLD_FILE="$PROJECT_DIR/worlds/$WORLD"
@@ -106,29 +91,23 @@ else
     print_error "World file not found: $WORLD"
     exit 1
 fi
-
 if [ -f "$PROJECT_DIR/scripts/generate_world.py" ]; then
     python3 "$PROJECT_DIR/scripts/generate_world.py"
 fi
-
 print_info "==================================="
 print_info "  UAV-UGV Simulation"
 print_info "==================================="
 print_info "World:    $WORLD_FILE"
 print_info "Mission:  $MISSION"
 echo ""
-
 GZ_DEFAULT_GUI="/usr/share/gz/gz-sim8/gui/gui.config"
 GZ_USER_GUI="$HOME/.gz/sim/8/gui.config"
 CAMERA_PLUGINS="$PROJECT_DIR/config/gui.config"
-
 rm -rf /tmp/gz-* /tmp/gazebo-* 2>/dev/null
-
 mkdir -p "$HOME/.gz/sim/8"
 cp "$GZ_DEFAULT_GUI" "$GZ_USER_GUI"
 sed -i 's|<start_paused>true</start_paused>|<start_paused>false</start_paused>|' "$GZ_USER_GUI"
 sed -i 's|show_again="true"|show_again="false"|' "$GZ_USER_GUI"
-
 cat << 'EOF' >> "$GZ_USER_GUI"
 <plugin filename="VideoRecorder" name="VideoRecorder">
   <gz-gui>
@@ -141,7 +120,6 @@ cat << 'EOF' >> "$GZ_USER_GUI"
     <property key="showTitleBar" type="bool">false</property>
     <property key="cardBackground" type="string">#777777</property>
   </gz-gui>
- 
   <record_video>
     <use_sim_time>true</use_sim_time>
     <lockstep>true</lockstep>
@@ -149,58 +127,45 @@ cat << 'EOF' >> "$GZ_USER_GUI"
   </record_video>
 </plugin>
 EOF
-
 print_info "[1/4] Starting Gazebo ..."
 gz sim -v4 -r "$WORLD_FILE" &
 GZ_PID=$!
 sleep 10
-
 if ! kill -0 $GZ_PID 2>/dev/null; then
     print_error "Gazebo failed to start"
     exit 1
 fi
 print_success "Gazebo running (PID: $GZ_PID)"
-
 print_info "[2/4] Starting ArduPilot SITL ..."
 cd ~/ardupilot
-
 SITL_ARGS="-v ArduCopter -f gazebo-iris --model JSON -I0"
 SITL_ARGS="$SITL_ARGS --no-mavproxy"
-
 PARAM_FILE="$PROJECT_DIR/config/ardupilot_gps_denied.parm"
 if [ -f "$PARAM_FILE" ]; then
     print_info "Loading params: $PARAM_FILE"
     SITL_ARGS="$SITL_ARGS --add-param-file $PARAM_FILE"
 fi
-
 sim_vehicle.py $SITL_ARGS &
 SITL_PID=$!
-
 print_info "Waiting for SITL (~20s) ..."
 sleep 20
-
 if ! pgrep -f "arducopter" > /dev/null 2>&1; then
     print_error "ArduPilot SITL failed to start"
     exit 1
 fi
 print_success "ArduPilot SITL running (TCP 5760)"
-
 print_info "[3/4] Starting main.py ..."
 cd "$PROJECT_DIR"
 sleep 3
-
 MAIN_ARGS="--device sim --connection tcp:127.0.0.1:5760 --search $SEARCH"
 [ -n "$ALTITUDE" ] && MAIN_ARGS="$MAIN_ARGS --altitude $ALTITUDE"
-
 python3 src/main.py $MAIN_ARGS &
 MAIN_PID=$!
 print_success "main.py running (PID: $MAIN_PID)"
-
 print_info "[4/4] Starting camera viewer ..."
 python3 -W ignore:RuntimeWarning -m src.vision.camera_processor down &
 CAM_PID=$!
 print_success "Camera viewer running (PID: $CAM_PID)"
-
 print_info ""
 print_info "==================================="
 print_info "  Simulation Running"
@@ -216,11 +181,8 @@ print_info "  target tag, dispatches UGV, lands on UGV"
 print_info "  Search: $SEARCH"
 print_info "  Press Ctrl+C to stop"
 print_info "==================================="
-
 wait $MAIN_PID 2>/dev/null
-
 cleanup_sitl
-
 print_info ""
 print_info "=================================="
 print_info "  Search Complete!"
@@ -229,5 +191,4 @@ print_info "Gazebo GUI still open."
 print_info "Press Ctrl+C to exit."
 print_info "=================================="
 print_info ""
-
 wait $GZ_PID 2>/dev/null

scripts/run_simulation.sh

@@ -1,40 +1,22 @@
 #!/bin/bash
-# =============================================================================
-# UAV-UGV Simulation - Run Script
-# =============================================================================
-# Launches Gazebo with ArduPilot SITL for GPS-denied navigation testing
-#
-# Usage:
-#   ./scripts/run_simulation.sh                     # Default world
-#   ./scripts/run_simulation.sh --world runway      # Runway world
-#   ./scripts/run_simulation.sh --software-render   # For WSL/no GPU
-# =============================================================================
-
 set -e
-
 RED='\033[0;31m'
 GREEN='\033[0;32m'
 YELLOW='\033[1;33m'
 BLUE='\033[0;34m'
 NC='\033[0m'
-
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
-
 echo -e "${BLUE}==========================================${NC}"
 echo -e "${BLUE}  UAV-UGV Simulation${NC}"
 echo -e "${BLUE}  GPS-Denied Navigation with Geofencing${NC}"
 echo -e "${BLUE}==========================================${NC}"
 echo ""
-
-# Detect WSL
 IS_WSL=false
 if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
     IS_WSL=true
     echo -e "${YELLOW}Running in WSL environment${NC}"
 fi
-
-# Detect ROS distro
 ROS_DISTRO=""
 for distro in jazzy humble iron galactic; do
     if [ -d "/opt/ros/$distro" ]; then
@@ -42,22 +24,16 @@ for distro in jazzy humble iron galactic; do
         break
     fi
 done
-
 if [ -n "$ROS_DISTRO" ]; then
     source /opt/ros/${ROS_DISTRO}/setup.bash
     echo -e "${GREEN}ROS 2: $ROS_DISTRO${NC}"
 fi
-
-# ArduPilot paths
 ARDUPILOT_HOME="${ARDUPILOT_HOME:-$HOME/ardupilot}"
 ARDUPILOT_GZ="${ARDUPILOT_GZ:-$HOME/ardupilot_gazebo}"
-
-# Parse arguments
 WORLD_NAME="iris_runway"
 VEHICLE="ArduCopter"
 USE_SOFTWARE_RENDER=false
 INSTANCE=0
-
 while [[ $# -gt 0 ]]; do
     case $1 in
         --world)
@@ -96,16 +72,11 @@ while [[ $# -gt 0 ]]; do
             ;;
     esac
 done
-
-# Setup Gazebo paths
 export GZ_SIM_RESOURCE_PATH="$PROJECT_DIR/models:$PROJECT_DIR/worlds:${GZ_SIM_RESOURCE_PATH:-}"
-
 if [ -d "$ARDUPILOT_GZ" ]; then
     export GZ_SIM_SYSTEM_PLUGIN_PATH="$ARDUPILOT_GZ/build:${GZ_SIM_SYSTEM_PLUGIN_PATH:-}"
     export GZ_SIM_RESOURCE_PATH="$ARDUPILOT_GZ/models:$ARDUPILOT_GZ/worlds:$GZ_SIM_RESOURCE_PATH"
 fi
-
-# Find world file
 WORLD_FILE=""
 if [ -f "$PROJECT_DIR/worlds/${WORLD_NAME}.sdf" ]; then
     WORLD_FILE="$PROJECT_DIR/worlds/${WORLD_NAME}.sdf"
@@ -123,8 +94,6 @@ else
     ls "$ARDUPILOT_GZ/worlds/"*.sdf 2>/dev/null | xargs -I {} basename {} .sdf || echo "    (none)"
     exit 1
 fi
-
-# WSL/Display setup
 if $IS_WSL; then
     if [ -z "$DISPLAY" ]; then
         if [ -d "/mnt/wslg" ]; then
@@ -135,75 +104,52 @@ if $IS_WSL; then
     fi
     echo -e "${BLUE}DISPLAY: $DISPLAY${NC}"
 fi
-
 if $USE_SOFTWARE_RENDER; then
     echo -e "${YELLOW}Using software rendering${NC}"
     export LIBGL_ALWAYS_SOFTWARE=1
     export MESA_GL_VERSION_OVERRIDE=3.3
 fi
-
-# Cleanup function
 cleanup() {
     echo ""
     echo -e "${YELLOW}Shutting down simulation...${NC}"
-    
     pkill -f "sim_vehicle.py" 2>/dev/null || true
     pkill -f "mavproxy" 2>/dev/null || true
     pkill -f "arducopter" 2>/dev/null || true
     pkill -f "ardurover" 2>/dev/null || true
-    pkill -f "ruby" 2>/dev/null || true  # gz sim uses ruby
+    pkill -f "ruby" 2>/dev/null || true
     pkill -f "gz sim" 2>/dev/null || true
-    
     sleep 2
     echo -e "${GREEN}Cleanup complete.${NC}"
 }
-
 trap cleanup EXIT INT TERM
-
-# Check ArduPilot installation
 if [ ! -d "$ARDUPILOT_HOME" ]; then
     echo -e "${RED}ERROR: ArduPilot not found at $ARDUPILOT_HOME${NC}"
     echo "Run setup.sh first to install ArduPilot"
     exit 1
 fi
-
-# Check Gazebo plugin
 if [ ! -f "$ARDUPILOT_GZ/build/libArduPilotPlugin.so" ]; then
     echo -e "${RED}ERROR: ArduPilot Gazebo plugin not found${NC}"
     echo "Run setup.sh first to build the plugin"
     exit 1
 fi
-
 echo ""
 echo -e "${GREEN}Configuration:${NC}"
 echo "  Vehicle: $VEHICLE"
 echo "  World: $(basename $WORLD_FILE .sdf)"
 echo "  Instance: $INSTANCE"
 echo ""
-
-# Start Gazebo in background
 echo -e "${GREEN}Starting Gazebo...${NC}"
 gz sim -v4 -r "$WORLD_FILE" &
 GZ_PID=$!
-
 sleep 5
-
-# Check if Gazebo started
 if ! kill -0 $GZ_PID 2>/dev/null; then
     echo -e "${RED}ERROR: Gazebo failed to start${NC}"
     exit 1
 fi
-
 echo -e "${GREEN}Gazebo running (PID: $GZ_PID)${NC}"
-
-# Start ArduPilot SITL
 echo -e "${GREEN}Starting ArduPilot SITL...${NC}"
 cd "$ARDUPILOT_HOME"
-
 export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest:$HOME/.local/bin:$HOME/.local/pipx/venvs/mavproxy/bin
-
-# Run sim_vehicle.py without --console to keep it running
-# The --no-mavproxy flag runs just the SITL, then we start mavproxy separately
 sim_vehicle.py \
     -v $VEHICLE \
     -f gazebo-iris \
@@ -212,11 +158,7 @@ sim_vehicle.py \
     --no-mavproxy \
     &
 SITL_PID=$!
-
-# Wait for SITL to initialize
 sleep 8
-
-# Start MAVProxy in foreground so user can interact with it
 echo ""
 echo -e "${GREEN}==========================================${NC}"
 echo -e "${GREEN}  Simulation Running${NC}"
@@ -227,17 +169,15 @@ echo "  - Gazebo: PID $GZ_PID"
 echo "  - ArduPilot SITL: PID $SITL_PID"
 echo ""
 echo -e "${BLUE}MAVProxy Commands:${NC}"
-echo "  mode guided      # Switch to GUIDED mode"
+echo "  mode guided
-echo "  arm throttle     # Arm the drone"
+echo "  arm throttle
-echo "  takeoff 5        # Takeoff to 5 meters"
+echo "  takeoff 5
-echo "  guided 10 5 -10  # Fly to position (N, E, D)"
+echo "  guided 10 5 -10
-echo "  rtl              # Return to launch"
+echo "  rtl
-echo "  land             # Land"
+echo "  land
 echo ""
 echo -e "${YELLOW}MAVProxy starting... (Ctrl+C to exit)${NC}"
 echo ""
-
-# Start MAVProxy in foreground - user can type commands directly
 mavproxy.py \
     --master tcp:127.0.0.1:5760 \
     --sitl 127.0.0.1:5501 \

scripts/setup_gazebo_nvidia.sh

@@ -1,34 +1,19 @@
 #!/bin/bash
-# Setup script for NVIDIA GPU with Gazebo
-
 set -e
-
 echo "Setting up NVIDIA GPU for Gazebo..."
-
-# Check if NVIDIA GPU is available
 if ! command -v nvidia-smi &> /dev/null; then
     echo "NVIDIA driver not found. Please install NVIDIA drivers first."
     exit 1
 fi
-
 nvidia-smi
-
-# Set environment variables for NVIDIA
 export __NV_PRIME_RENDER_OFFLOAD=1
 export __GLX_VENDOR_LIBRARY_NAME=nvidia
-
-# For Gazebo Harmonic
 export LIBGL_ALWAYS_SOFTWARE=0
-
-# Create persistent config
 cat >> ~/.bashrc << 'EOF'
-
-# NVIDIA GPU for Gazebo
 export __NV_PRIME_RENDER_OFFLOAD=1
 export __GLX_VENDOR_LIBRARY_NAME=nvidia
 export LIBGL_ALWAYS_SOFTWARE=0
 EOF
-
 echo ""
 echo "NVIDIA setup complete!"
 echo "Please restart your terminal or run: source ~/.bashrc"

scripts/uninstall.sh

@@ -1,38 +1,20 @@
 #!/bin/bash
-# =============================================================================
-# UAV-UGV Simulation - Uninstall Script
-# =============================================================================
-# Removes ArduPilot, ardupilot_gazebo, and project files
-# Does NOT remove ROS 2 or Gazebo (system packages)
-#
-# Usage:
-#   ./scripts/uninstall.sh           # Remove ArduPilot and plugin only
-#   ./scripts/uninstall.sh --all     # Remove everything including project
-# =============================================================================
-
 set -e
-
 RED='\033[0;31m'
 GREEN='\033[0;32m'
 YELLOW='\033[1;33m'
 BLUE='\033[0;34m'
 NC='\033[0m'
-
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
-
 ARDUPILOT_HOME="$HOME/ardupilot"
 ARDUPILOT_GZ="$HOME/ardupilot_gazebo"
-
 echo -e "${BLUE}==========================================${NC}"
 echo -e "${BLUE}  UAV-UGV Simulation - Uninstall${NC}"
 echo -e "${BLUE}==========================================${NC}"
 echo ""
-
-# Parse arguments
 REMOVE_ALL=false
 FORCE=false
-
 for arg in "$@"; do
     case $arg in
         --all)
@@ -54,8 +36,6 @@ for arg in "$@"; do
             ;;
     esac
 done
-
-# Confirmation
 if [ "$FORCE" = false ]; then
     echo "This will remove:"
     echo "  - ArduPilot SITL ($ARDUPILOT_HOME)"
@@ -73,10 +53,7 @@ if [ "$FORCE" = false ]; then
         exit 0
     fi
 fi
-
 echo ""
-
-# Kill running processes
 echo -e "${YELLOW}Stopping running processes...${NC}"
 pkill -f "sim_vehicle.py" 2>/dev/null || true
 pkill -f "mavproxy" 2>/dev/null || true
@@ -86,8 +63,6 @@ pkill -f "gz sim" 2>/dev/null || true
 pkill -f "gzserver" 2>/dev/null || true
 pkill -f "gzclient" 2>/dev/null || true
 sleep 1
-
-# Remove ArduPilot
 if [ -d "$ARDUPILOT_HOME" ]; then
     echo -e "${YELLOW}Removing ArduPilot ($ARDUPILOT_HOME)...${NC}"
     rm -rf "$ARDUPILOT_HOME"
@@ -95,8 +70,6 @@ if [ -d "$ARDUPILOT_HOME" ]; then
 else
     echo "ArduPilot not found at $ARDUPILOT_HOME"
 fi
-
-# Remove ardupilot_gazebo
 if [ -d "$ARDUPILOT_GZ" ]; then
     echo -e "${YELLOW}Removing ardupilot_gazebo ($ARDUPILOT_GZ)...${NC}"
     rm -rf "$ARDUPILOT_GZ"
@@ -104,33 +77,22 @@ if [ -d "$ARDUPILOT_GZ" ]; then
 else
     echo "ardupilot_gazebo not found at $ARDUPILOT_GZ"
 fi
-
-# Remove Python venv
 if [ -d "$PROJECT_DIR/venv" ]; then
     echo -e "${YELLOW}Removing Python virtual environment...${NC}"
     rm -rf "$PROJECT_DIR/venv"
     echo -e "${GREEN}Removed venv${NC}"
 fi
-
-# Remove generated files
 rm -f "$PROJECT_DIR/activate_venv.sh" 2>/dev/null || true
 rm -f "$PROJECT_DIR/wsl_env.sh" 2>/dev/null || true
-
-# Remove ArduPilot environment files
 rm -f "$HOME/.ardupilot_env" 2>/dev/null || true
-
-# Remove pip user packages
 echo -e "${YELLOW}Removing user pip packages (mavproxy, pymavlink)...${NC}"
 pip3 uninstall -y mavproxy pymavlink pexpect 2>/dev/null || true
-
-# Remove project directory if --all
 if [ "$REMOVE_ALL" = true ]; then
     echo -e "${YELLOW}Removing project directory ($PROJECT_DIR)...${NC}"
     cd "$HOME"
     rm -rf "$PROJECT_DIR"
     echo -e "${GREEN}Removed project directory${NC}"
 fi
-
 echo ""
 echo -e "${GREEN}==========================================${NC}"
 echo -e "${GREEN}  Uninstall Complete${NC}"

setup.sh

@@ -1,24 +1,11 @@
 #!/bin/bash
-# =============================================================================
-# UAV-UGV Simulation - Complete Installation Script
-# =============================================================================
-# Installs everything needed for GPS-denied navigation simulation
-# Compatible with Ubuntu 22.04/24.04 and WSL2
-#
-# Usage:
-#   ./setup.sh                    # Full installation
-#   ./setup.sh --skip-ardupilot   # Skip ArduPilot (Gazebo only)
-# =============================================================================
-
 set -e
-
 RED='\033[0;31m'
 GREEN='\033[0;32m'
 YELLOW='\033[1;33m'
 BLUE='\033[0;34m'
 CYAN='\033[0;36m'
 NC='\033[0m'
-
 print_header() {
     echo ""
     echo -e "${BLUE}==========================================${NC}"
@@ -26,49 +13,34 @@ print_header() {
     echo -e "${BLUE}==========================================${NC}"
     echo ""
 }
-
 print_step() {
     echo -e "${GREEN}[$1/$TOTAL_STEPS] $2${NC}"
 }
-
 print_info() {
     echo -e "${CYAN}INFO: $1${NC}"
 }
-
 print_warning() {
     echo -e "${YELLOW}WARNING: $1${NC}"
 }
-
 print_error() {
     echo -e "${RED}ERROR: $1${NC}"
 }
-
-# Get script directory
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 cd "$SCRIPT_DIR"
-
-# Deactivate existing venv if active to avoid conflicts
 if [ -n "$VIRTUAL_ENV" ]; then
     deactivate 2>/dev/null || true
 fi
-
-# ArduPilot directories
 ARDUPILOT_HOME="$HOME/ardupilot"
 ARDUPILOT_GZ="$HOME/ardupilot_gazebo"
-
-# Detect environment
 detect_environment() {
     IS_WSL=false
     IS_WSL2=false
-    
     if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
         IS_WSL=true
         if grep -qi "wsl2" /proc/version 2>/dev/null || [ -f /run/WSL ]; then
             IS_WSL2=true
         fi
     fi
-    
-    # Detect Ubuntu version
     if [ -f /etc/os-release ]; then
         . /etc/os-release
         UBUNTU_VERSION="$VERSION_ID"
@@ -77,8 +49,6 @@ detect_environment() {
         UBUNTU_VERSION="22.04"
         UBUNTU_CODENAME="jammy"
     fi
-    
-    # Determine ROS distro and compatible repository codename
     if [ "$UBUNTU_VERSION" = "24.04" ]; then
         ROS_DISTRO="jazzy"
         ROS_UBUNTU_CODENAME="noble"
@@ -86,7 +56,6 @@ detect_environment() {
         ROS_DISTRO="humble"
         ROS_UBUNTU_CODENAME="jammy"
     else
-        # For newer versions, use latest available
         UBUNTU_MAJOR=$(echo "$UBUNTU_VERSION" | cut -d. -f1)
         if [ "$UBUNTU_MAJOR" -ge 24 ] 2>/dev/null; then
             ROS_DISTRO="jazzy"
@@ -97,40 +66,29 @@ detect_environment() {
         fi
     fi
 }
-
-# Parse arguments
 INSTALL_ARDUPILOT=true
 for arg in "$@"; do
     if [ "$arg" = "--skip-ardupilot" ]; then
         INSTALL_ARDUPILOT=false
     fi
 done
-
 detect_environment
-
 print_header "UAV-UGV Simulation - Complete Setup"
 echo "GPS-Denied Navigation with Geofencing"
 echo ""
-
 echo -e "${CYAN}Detected Environment:${NC}"
 echo "  Ubuntu: $UBUNTU_VERSION ($UBUNTU_CODENAME)"
 echo "  WSL: $IS_WSL | WSL2: $IS_WSL2"
 echo "  ROS 2 Target: $ROS_DISTRO"
 echo "  ArduPilot: $INSTALL_ARDUPILOT"
 echo ""
-
 if [ "$INSTALL_ARDUPILOT" = true ]; then
     TOTAL_STEPS=10
 else
     TOTAL_STEPS=7
 fi
 STEP=1
-
-# ============================================================================
-# STEP 1: System Update & Dependencies
-# ============================================================================
 print_step $STEP "Installing system dependencies"
-
 sudo apt-get update
 sudo apt-get install -y \
     curl \
@@ -165,9 +123,10 @@ sudo apt-get install -y \
     ccache \
     gawk \
     libtool-bin \
-    netcat-openbsd
+    netcat-openbsd \
-
+    ffmpeg \
-# WSL-specific packages
+    xdotool \
+    x11-utils
 if $IS_WSL; then
     print_info "Installing WSL GUI support packages"
     sudo apt-get install -y \
@@ -178,24 +137,13 @@ if $IS_WSL; then
         libgl1-mesa-glx
 fi
 ((STEP++))
-
-# ============================================================================
-# STEP 2: Install ROS 2
-# ============================================================================
 print_step $STEP "Installing ROS 2 $ROS_DISTRO"
-
-# Add ROS 2 GPG key
 sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
     -o /usr/share/keyrings/ros-archive-keyring.gpg
-
-# Add repository using compatible Ubuntu codename
 echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
     http://packages.ros.org/ros2/ubuntu $ROS_UBUNTU_CODENAME main" | \
     sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
-
 sudo apt-get update
-
-# Install ROS 2
 sudo apt-get install -y \
     ros-${ROS_DISTRO}-ros-base \
     ros-${ROS_DISTRO}-geometry-msgs \
@@ -206,35 +154,21 @@ sudo apt-get install -y \
     print_error "Failed to install ROS 2 $ROS_DISTRO"
     exit 1
 }
-
-# Install ros-gz bridge
 sudo apt-get install -y ros-${ROS_DISTRO}-ros-gz || \
     print_warning "Could not install ros-gz bridge"
-
 print_info "ROS 2 $ROS_DISTRO installed"
 ((STEP++))
-
-# ============================================================================
-# STEP 3: Install Gazebo (Harmonic or Garden)
-# ============================================================================
 print_step $STEP "Installing Gazebo"
-
-# Add Gazebo repository
 GZ_UBUNTU_CODENAME="$ROS_UBUNTU_CODENAME"
 sudo wget -q https://packages.osrfoundation.org/gazebo.gpg \
     -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg 2>/dev/null || true
-
 echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] \
     http://packages.osrfoundation.org/gazebo/ubuntu-stable $GZ_UBUNTU_CODENAME main" | \
     sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
-
 sudo apt-get update
-
-# Try Gazebo Harmonic first, then Garden
 GZ_VERSION=""
 if sudo apt-get install -y gz-harmonic 2>/dev/null; then
     GZ_VERSION="harmonic"
-    # Install Harmonic development packages for building plugins
     sudo apt-get install -y \
         libgz-cmake3-dev \
         libgz-sim8-dev \
@@ -247,14 +181,12 @@ if sudo apt-get install -y gz-harmonic 2>/dev/null; then
         libgz-msgs10-dev \
         rapidjson-dev \
         2>/dev/null || print_warning "Some Gazebo dev packages may be missing"
-    # Install Gazebo Python bindings (needed for camera/vision scripts)
     sudo apt-get install -y \
         python3-gz-transport13 \
         python3-gz-msgs10 \
         2>/dev/null || print_warning "Gazebo Python bindings not available via apt"
 elif sudo apt-get install -y gz-garden 2>/dev/null; then
     GZ_VERSION="garden"
-    # Install Garden development packages
     sudo apt-get install -y \
         libgz-cmake3-dev \
         libgz-sim7-dev \
@@ -267,7 +199,6 @@ elif sudo apt-get install -y gz-garden 2>/dev/null; then
         libgz-msgs9-dev \
         rapidjson-dev \
         2>/dev/null || print_warning "Some Gazebo dev packages may be missing"
-    # Install Gazebo Python bindings for Garden
     sudo apt-get install -y \
         python3-gz-transport12 \
         python3-gz-msgs9 \
@@ -276,52 +207,29 @@ else
     print_warning "Could not install Gazebo Harmonic/Garden"
     GZ_VERSION="none"
 fi
-
 if command -v gz &> /dev/null; then
     print_info "Gazebo $GZ_VERSION installed"
 else
     print_warning "Gazebo command not found"
 fi
 ((STEP++))
-
-# ============================================================================
-# STEP 4: Python Virtual Environment
-# ============================================================================
 print_step $STEP "Setting up Python environment"
-
 if [ -d "$SCRIPT_DIR/venv" ]; then
     rm -rf "$SCRIPT_DIR/venv"
 fi
-
-# --system-site-packages is required so the venv can access
-# Gazebo Python bindings (gz.transport13, gz.msgs10) which are
-# installed as system packages and cannot be pip-installed.
 python3 -m venv --system-site-packages "$SCRIPT_DIR/venv"
 source "$SCRIPT_DIR/venv/bin/activate"
 pip install --upgrade pip
-
 if [ -f "$SCRIPT_DIR/requirements.txt" ]; then
     pip install -r "$SCRIPT_DIR/requirements.txt" || print_warning "Some Python packages failed"
 else
     pip install numpy opencv-python scipy shapely filterpy transforms3d pymavlink pexpect future
 fi
-
 deactivate
 ((STEP++))
-
-# ============================================================================
-# STEP 5: Create Activation Script
-# ============================================================================
 print_step $STEP "Creating activation script"
-
 cat > "$SCRIPT_DIR/activate_venv.sh" << 'ACTIVATE_EOF'
-#!/bin/bash
-# UAV-UGV Simulation - Environment Activation
-# Usage: source activate_venv.sh
-
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
-
-# ROS 2 Setup
 if [ -f "/opt/ros/jazzy/setup.bash" ]; then
     source /opt/ros/jazzy/setup.bash
     ROS_VER="jazzy"
@@ -332,32 +240,20 @@ else
     echo "[WARN] ROS 2 not found in /opt/ros/"
     ROS_VER="none"
 fi
-
-# ArduPilot Environment
 export ARDUPILOT_HOME="$HOME/ardupilot"
 export PATH="$ARDUPILOT_HOME/Tools/autotest:$PATH"
 export PATH="$HOME/.local/bin:$PATH"
-
-# Deactivate existing venv
 if [ -n "$VIRTUAL_ENV" ]; then
     deactivate 2>/dev/null || true
 fi
-
-# Activate project venv
 if [ -f "$SCRIPT_DIR/venv/bin/activate" ]; then
     source "$SCRIPT_DIR/venv/bin/activate"
 fi
-
-# Gazebo paths (new Gazebo - Ignition/Harmonic)
 export GZ_SIM_RESOURCE_PATH="$SCRIPT_DIR/models:$SCRIPT_DIR/worlds:${GZ_SIM_RESOURCE_PATH:-}"
-
-# ArduPilot Gazebo plugin
 if [ -d "$HOME/ardupilot_gazebo/build" ]; then
     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"
 fi
-
-# WSL environment
 if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
     if [ -d "/mnt/wslg" ]; then
         export DISPLAY=:0
@@ -367,31 +263,20 @@ if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
     export LIBGL_ALWAYS_INDIRECT=0
     export MESA_GL_VERSION_OVERRIDE=3.3
 fi
-
 echo -e "\033[0;32mEnvironment activated (ROS 2 $ROS_VER)\033[0m"
 echo ""
 echo "Run simulation:  bash scripts/run_autonomous.sh --search spiral"
 ACTIVATE_EOF
-
 chmod +x "$SCRIPT_DIR/activate_venv.sh"
 ((STEP++))
-
-# ============================================================================
-# STEP 6: Create WSL Environment (if applicable)
-# ============================================================================
 print_step $STEP "Configuring environment files"
-
 if $IS_WSL; then
     cat > "$SCRIPT_DIR/wsl_env.sh" << 'WSLEOF'
-#!/bin/bash
-# WSL Environment for UAV-UGV Simulation
-
 if [ -d "/mnt/wslg" ]; then
     export DISPLAY=:0
 else
     export DISPLAY=$(cat /etc/resolv.conf 2>/dev/null | grep nameserver | awk '{print $2}'):0
 fi
-
 export LIBGL_ALWAYS_INDIRECT=0
 export MESA_GL_VERSION_OVERRIDE=3.3
 export MESA_GLSL_VERSION_OVERRIDE=330
@@ -400,56 +285,30 @@ WSLEOF
     chmod +x "$SCRIPT_DIR/wsl_env.sh"
 fi
 ((STEP++))
-
-# ============================================================================
-# ARDUPILOT INSTALLATION (if not skipped)
-# ============================================================================
 if [ "$INSTALL_ARDUPILOT" = true ]; then
-
-    # ========================================================================
-    # STEP 7: Clone and Setup ArduPilot
-    # ========================================================================
     print_step $STEP "Setting up ArduPilot SITL"
-    
     if [ ! -d "$ARDUPILOT_HOME" ]; then
         print_info "Cloning ArduPilot repository..."
         git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git "$ARDUPILOT_HOME"
     else
         print_info "ArduPilot directory already exists"
     fi
-    
     cd "$ARDUPILOT_HOME"
     git submodule update --init --recursive
-    
-    # Install ArduPilot prerequisites
     print_info "Installing ArduPilot prerequisites (this may take a while)..."
     Tools/environment_install/install-prereqs-ubuntu.sh -y || true
     . ~/.profile 2>/dev/null || true
-    
-    # Source ArduPilot environment
     [ -f "$HOME/.ardupilot_env" ] && source "$HOME/.ardupilot_env"
-    
     print_info "ArduPilot prerequisites installed"
     ((STEP++))
-    
-    # ========================================================================
-    # STEP 8: Build ArduCopter SITL
-    # ========================================================================
     print_step $STEP "Building ArduPilot SITL (this takes several minutes)"
-    
     cd "$ARDUPILOT_HOME"
     ./waf configure --board sitl
     ./waf copter
     ./waf rover
-    
     print_info "ArduPilot SITL built"
     ((STEP++))
-    
-    # ========================================================================
-    # STEP 9: Build ArduPilot Gazebo Plugin
-    # ========================================================================
     print_step $STEP "Building ArduPilot Gazebo plugin"
-    
     if [ ! -d "$ARDUPILOT_GZ" ]; then
         print_info "Cloning ardupilot_gazebo repository..."
         git clone https://github.com/ArduPilot/ardupilot_gazebo.git "$ARDUPILOT_GZ"
@@ -458,46 +317,30 @@ if [ "$INSTALL_ARDUPILOT" = true ]; then
         cd "$ARDUPILOT_GZ"
         git pull origin main || true
     fi
-    
     cd "$ARDUPILOT_GZ"
     mkdir -p build && cd build
-    
-    # Deactivate venv so cmake finds system Python with ROS 2 ament packages
     deactivate 2>/dev/null || true
     cmake .. -DCMAKE_BUILD_TYPE=Release
     make -j$(nproc)
-    # Reactivate venv
     source "$SCRIPT_DIR/venv/bin/activate"
-    
     print_info "ArduPilot Gazebo plugin built"
-    
-    # Install MAVProxy using pipx (required for Ubuntu 23.04+ PEP 668)
     print_info "Installing MAVProxy..."
     if pipx install MAVProxy --include-deps; then
         print_info "Injecting dependencies into MAVProxy venv..."
         pipx inject mavproxy future pexpect
     else
-        # Fallback: try pip with --break-system-packages
         pip3 install --user --break-system-packages pymavlink mavproxy 2>/dev/null || \
         pip3 install --user pymavlink mavproxy 2>/dev/null || \
         print_warning "MAVProxy installation failed - install manually"
     fi
     ((STEP++))
-    
-    # ========================================================================
-    # STEP 10: Verify Installation
-    # ========================================================================
     print_step $STEP "Verifying installation"
-    
     export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest:$HOME/.local/bin
-    
     echo ""
     command -v sim_vehicle.py &> /dev/null && echo "[OK] sim_vehicle.py" || echo "[WARN] sim_vehicle.py not found"
     command -v gz &> /dev/null && echo "[OK] Gazebo (gz)" || echo "[WARN] Gazebo not found"
     command -v mavproxy.py &> /dev/null && echo "[OK] MAVProxy" || echo "[WARN] MAVProxy not in PATH"
     [ -f "$ARDUPILOT_GZ/build/libArduPilotPlugin.so" ] && echo "[OK] ArduPilot Gazebo plugin" || echo "[WARN] Plugin not built"
-    
-    # Verify Gazebo Python bindings are accessible from venv
     source "$SCRIPT_DIR/venv/bin/activate"
     python3 -c "from gz.transport13 import Node; print('[OK] gz.transport13 Python bindings')" 2>/dev/null || \
         echo "[WARN] gz.transport13 Python bindings not found"
@@ -509,19 +352,9 @@ if [ "$INSTALL_ARDUPILOT" = true ]; then
         echo "[WARN] OpenCV not found in venv"
     deactivate
     ((STEP++))
-
 fi
-
-# ============================================================================
-# Make scripts executable
-# ============================================================================
 chmod +x "$SCRIPT_DIR/scripts/"*.sh 2>/dev/null || true
-
-# ============================================================================
-# COMPLETE
-# ============================================================================
 print_header "Installation Complete!"
-
 echo -e "${GREEN}Components installed:${NC}"
 echo "  - ROS 2 $ROS_DISTRO"
 echo "  - Gazebo $GZ_VERSION"
@@ -531,14 +364,12 @@ if [ "$INSTALL_ARDUPILOT" = true ]; then
 fi
 echo "  - Python dependencies"
 echo ""
-
 if $IS_WSL; then
     echo -e "${YELLOW}WSL Notes:${NC}"
     echo "  - GUI requires WSLg (Win11) or VcXsrv (Win10)"
     echo "  - Use --software-render if graphics are slow"
     echo ""
 fi
-
 echo -e "${CYAN}To run the simulation:${NC}"
 echo ""
 echo "  cd $SCRIPT_DIR"

src/__init__.py

@@ -0,0 +1 @@
+

src/control/__init__.py

@@ -0,0 +1 @@
+

src/control/mission_planner.py

@@ -1,236 +0,0 @@
-#!/usr/bin/env python3
-"""Mission Planner - Coordinates missions using local waypoints."""
-
-import rclpy
-from rclpy.node import Node
-from geometry_msgs.msg import PoseStamped
-from nav_msgs.msg import Path
-from std_msgs.msg import String, Bool
-import yaml
-from enum import Enum
-
-
-class MissionState(Enum):
-    IDLE = 0
-    LOADING = 1
-    EXECUTING = 2
-    PAUSED = 3
-    COMPLETED = 4
-    ABORTED = 5
-
-
-class MissionPlanner(Node):
-    
-    def __init__(self):
-        super().__init__('mission_planner')
-        
-        self.declare_parameter('mission_file', '')
-        
-        self.mission = []
-        self.current_action_idx = 0
-        self.state = MissionState.IDLE
-        
-        self.uav_goal_reached = False
-        self.ugv_goal_reached = False
-        
-        self.command_sub = self.create_subscription(
-            String, '/mission/command', self.command_callback, 10)
-        
-        self.uav_status_sub = self.create_subscription(
-            String, '/uav/controller/status', self.uav_status_callback, 10)
-        
-        self.ugv_status_sub = self.create_subscription(
-            String, '/ugv/controller/status', self.ugv_status_callback, 10)
-        
-        self.uav_reached_sub = self.create_subscription(
-            Bool, '/uav/waypoint_follower/waypoint_reached', self.uav_reached_callback, 10)
-        
-        self.ugv_reached_sub = self.create_subscription(
-            Bool, '/ugv/goal_reached', self.ugv_reached_callback, 10)
-        
-        self.uav_cmd_pub = self.create_publisher(String, '/uav/controller/command', 10)
-        self.ugv_cmd_pub = self.create_publisher(String, '/ugv/controller/command', 10)
-        self.uav_waypoint_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10)
-        self.ugv_goal_pub = self.create_publisher(PoseStamped, '/ugv/goal_pose', 10)
-        self.uav_path_pub = self.create_publisher(Path, '/waypoint_follower/set_path', 10)
-        self.status_pub = self.create_publisher(String, '/mission/status', 10)
-        
-        self.mission_timer = self.create_timer(0.5, self.mission_loop)
-        
-        self.get_logger().info('Mission Planner Started - LOCAL coordinates only')
-    
-    def command_callback(self, msg):
-        cmd = msg.data.lower().split(':')
-        action = cmd[0]
-        
-        if action == 'load':
-            if len(cmd) > 1:
-                self.load_mission_file(cmd[1])
-            else:
-                self.load_demo_mission()
-        elif action == 'start':
-            self.start_mission()
-        elif action == 'pause':
-            self.state = MissionState.PAUSED
-        elif action == 'resume':
-            self.state = MissionState.EXECUTING
-        elif action == 'abort':
-            self.abort_mission()
-        elif action == 'clear':
-            self.mission = []
-            self.current_action_idx = 0
-            self.state = MissionState.IDLE
-    
-    def uav_status_callback(self, msg):
-        pass
-    
-    def ugv_status_callback(self, msg):
-        pass
-    
-    def uav_reached_callback(self, msg):
-        if msg.data:
-            self.uav_goal_reached = True
-    
-    def ugv_reached_callback(self, msg):
-        if msg.data:
-            self.ugv_goal_reached = True
-    
-    def load_demo_mission(self):
-        self.mission = [
-            {'type': 'uav_command', 'command': 'arm'},
-            {'type': 'uav_command', 'command': 'takeoff'},
-            {'type': 'wait', 'duration': 3.0},
-            {'type': 'uav_waypoint', 'x': 10.0, 'y': 0.0, 'z': 5.0},
-            {'type': 'uav_waypoint', 'x': 10.0, 'y': 10.0, 'z': 5.0},
-            {'type': 'ugv_goal', 'x': 5.0, 'y': 5.0},
-            {'type': 'wait_for_vehicles'},
-            {'type': 'uav_waypoint', 'x': 0.0, 'y': 0.0, 'z': 5.0},
-            {'type': 'ugv_goal', 'x': 0.0, 'y': 0.0},
-            {'type': 'wait_for_vehicles'},
-            {'type': 'uav_command', 'command': 'land'},
-        ]
-        self.current_action_idx = 0
-        self.state = MissionState.IDLE
-        self.get_logger().info(f'Demo mission loaded: {len(self.mission)} actions')
-    
-    def load_mission_file(self, filepath):
-        try:
-            with open(filepath, 'r') as f:
-                data = yaml.safe_load(f)
-                self.mission = data.get('mission', [])
-                self.current_action_idx = 0
-                self.state = MissionState.IDLE
-                self.get_logger().info(f'Mission loaded from {filepath}')
-        except Exception as e:
-            self.get_logger().error(f'Failed to load mission: {e}')
-    
-    def start_mission(self):
-        if len(self.mission) == 0:
-            self.get_logger().warn('No mission loaded')
-            return
-        
-        self.current_action_idx = 0
-        self.state = MissionState.EXECUTING
-        self.uav_goal_reached = False
-        self.ugv_goal_reached = False
-        self.get_logger().info('Mission started')
-    
-    def abort_mission(self):
-        self.state = MissionState.ABORTED
-        
-        stop_msg = String()
-        stop_msg.data = 'hold'
-        self.uav_cmd_pub.publish(stop_msg)
-        
-        stop_msg.data = 'stop'
-        self.ugv_cmd_pub.publish(stop_msg)
-        
-        self.get_logger().warn('Mission aborted')
-    
-    def mission_loop(self):
-        status_msg = String()
-        status_msg.data = f'{self.state.name}|{self.current_action_idx}/{len(self.mission)}'
-        self.status_pub.publish(status_msg)
-        
-        if self.state != MissionState.EXECUTING:
-            return
-        
-        if self.current_action_idx >= len(self.mission):
-            self.state = MissionState.COMPLETED
-            self.get_logger().info('Mission completed!')
-            return
-        
-        action = self.mission[self.current_action_idx]
-        completed = self.execute_action(action)
-        
-        if completed:
-            self.current_action_idx += 1
-            self.uav_goal_reached = False
-            self.ugv_goal_reached = False
-    
-    def execute_action(self, action):
-        action_type = action.get('type', '')
-        
-        if action_type == 'uav_command':
-            cmd_msg = String()
-            cmd_msg.data = action['command']
-            self.uav_cmd_pub.publish(cmd_msg)
-            return True
-        
-        elif action_type == 'ugv_command':
-            cmd_msg = String()
-            cmd_msg.data = action['command']
-            self.ugv_cmd_pub.publish(cmd_msg)
-            return True
-        
-        elif action_type == 'uav_waypoint':
-            pose = PoseStamped()
-            pose.header.stamp = self.get_clock().now().to_msg()
-            pose.header.frame_id = 'odom'
-            pose.pose.position.x = float(action['x'])
-            pose.pose.position.y = float(action['y'])
-            pose.pose.position.z = float(action['z'])
-            pose.pose.orientation.w = 1.0
-            self.uav_waypoint_pub.publish(pose)
-            return self.uav_goal_reached
-        
-        elif action_type == 'ugv_goal':
-            pose = PoseStamped()
-            pose.header.stamp = self.get_clock().now().to_msg()
-            pose.header.frame_id = 'odom'
-            pose.pose.position.x = float(action['x'])
-            pose.pose.position.y = float(action['y'])
-            pose.pose.orientation.w = 1.0
-            self.ugv_goal_pub.publish(pose)
-            return self.ugv_goal_reached
-        
-        elif action_type == 'wait':
-            if not hasattr(self, '_wait_start'):
-                self._wait_start = self.get_clock().now()
-            
-            elapsed = (self.get_clock().now() - self._wait_start).nanoseconds / 1e9
-            if elapsed >= action['duration']:
-                delattr(self, '_wait_start')
-                return True
-            return False
-        
-        elif action_type == 'wait_for_vehicles':
-            return self.uav_goal_reached and self.ugv_goal_reached
-        
-        return True
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = MissionPlanner()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

src/control/uav_controller.py

@@ -29,7 +29,6 @@ DEFAULT_WPNAV_SPEED_DN = 75
 DEFAULT_WPNAV_ACCEL_Z = 75
 DEFAULT_LOIT_SPEED = 150
 
-
 class Controller:
     HOLD_ALT = HOLD_ALT
     LOWEST_ALT = LOWEST_ALT
@@ -81,10 +80,6 @@ class Controller:
         self.update_state()
         return self.position.copy()
 
-    def get_altitude(self) -> float:
-        self.update_state()
-        return self.altitude
-
     def set_param(self, name: str, value: float):
         self.conn.mav.param_set_send(
             self.conn.target_system,
@@ -96,15 +91,15 @@ class Controller:
         self._drain_messages()
 
     def configure_ekf_fast_converge(self):
-        """Tune EKF parameters for faster initial convergence in SITL."""
+
         params = {
-            'EK3_CHECK_SCALE': 200,    # Relax EKF health checks (default 100)
+            'EK3_CHECK_SCALE': 200,
-            'EK3_GPS_CHECK':   0,      # Disable GPS preflight checks in SITL
+            'EK3_GPS_CHECK':   0,
-            'EK3_POSNE_M_NSE': 0.5,    # Trust GPS position more
+            'EK3_POSNE_M_NSE': 0.5,
-            'EK3_VELD_M_NSE':  0.2,    # Trust GPS vertical vel more (default 0.4)
+            'EK3_VELD_M_NSE':  0.2,
-            'EK3_VELNE_M_NSE': 0.1,    # Trust GPS horizontal vel more (default 0.3)
+            'EK3_VELNE_M_NSE': 0.1,
-            'INS_GYRO_CAL':    0,      # Skip gyro cal on boot (SITL doesn't need it)
+            'INS_GYRO_CAL':    0,
-            'ARMING_CHECK':    0,      # Disable ALL preflight checks (SITL only)
+            'ARMING_CHECK':    0,
         }
         for name, value in params.items():
             self.set_param(name, value)
@@ -131,15 +126,14 @@ class Controller:
         print("[UAV] Mode -> GUIDED_NOGPS")
 
     def wait_for_ekf(self, timeout: float = 120.0) -> bool:
-        """Wait until EKF has actual GPS-fused position estimate."""
+
         print("[UAV] Waiting for EKF position estimate ...")
         t0 = time.time()
         while time.time() - t0 < timeout:
             self._drain_messages()
             msg = self.conn.recv_match(type='EKF_STATUS_REPORT', blocking=True, timeout=2)
             if msg is not None:
-                # bit 0x08 = pos_horiz_abs (actual GPS-fused position)
+
-                # bit 0x20 = pred_pos_horiz_abs (predicted, not enough for arming)
                 pos_horiz_abs = bool(msg.flags & 0x08)
                 if pos_horiz_abs:
                     elapsed = int(time.time() - t0)
@@ -151,7 +145,6 @@ class Controller:
     def arm(self, retries: int = 15):
         self.set_mode_guided()
 
-        # Wait for EKF before burning through arm attempts
         if not self.wait_for_ekf(timeout=120.0):
             print("[UAV] Aborting arm: EKF never became healthy")
             return False
@@ -179,10 +172,6 @@ class Controller:
             time.sleep(2)
         return False
 
-    def disarm(self):
-        self.conn.arducopter_disarm()
-        print("[UAV] Disarmed")
-
     def takeoff(self, altitude: float = HOLD_ALT):
         self.conn.mav.command_long_send(
             self.conn.target_system,
@@ -199,36 +188,6 @@ class Controller:
             0, 89, 9, 0, 0, 0, 0, 0)
         print("[UAV] Landing (LAND mode)")
 
-    def move_pos_rel(self, x: float, y: float, z: float):
-        move_msg = MAVLink_set_position_target_local_ned_message(
-            int(perf_counter() * 1000),
-            self.conn.target_system,
-            self.conn.target_component,
-            MAV_FRAME_BODY_OFFSET_NED,
-            3576, x, y, z,
-            0, 0, 0, 0, 0, 0, 0, 0)
-        self.conn.mav.send(move_msg)
-
-    def move_pos_rel_yaw(self, x: float, y: float, z: float, yaw: float):
-        move_msg = MAVLink_set_position_target_local_ned_message(
-            int(perf_counter() * 1000),
-            self.conn.target_system,
-            self.conn.target_component,
-            MAV_FRAME_BODY_OFFSET_NED,
-            3576, x, y, z,
-            0, 0, 0, 0, 0, 0, yaw, 0)
-        self.conn.mav.send(move_msg)
-
-    def move_vel_rel_alt(self, vx: float, vy: float, z: float):
-        move_msg = MAVLink_set_position_target_local_ned_message(
-            int(perf_counter() * 1000),
-            self.conn.target_system,
-            self.conn.target_component,
-            MAV_FRAME_BODY_OFFSET_NED,
-            3555, 0, 0, z,
-            vx, vy, 0, 0, 0, 0, 0, 0)
-        self.conn.mav.send(move_msg)
-
     def move_local_ned(self, x: float, y: float, z: float):
         move_msg = MAVLink_set_position_target_local_ned_message(
             int(perf_counter() * 1000),
@@ -239,9 +198,6 @@ class Controller:
             0, 0, 0, 0, 0, 0, 0, 0)
         self.conn.mav.send(move_msg)
 
-    def distance(self, p1: Tuple, p2: Tuple) -> float:
-        return float(np.linalg.norm(np.asarray(p1) - np.asarray(p2)))
-
     def wait_altitude(self, target_alt: float, tolerance: float = 0.5,
                       timeout: float = 30.0):
         t0 = time.time()
@@ -276,9 +232,24 @@ class Controller:
               f"accel={wpnav_accel}cm/s²  up={wpnav_speed_up}cm/s  "
               f"dn={wpnav_speed_dn}cm/s")
 
-    def set_max_velocity(self, speed):
+def setup_ardupilot(ctrl: Controller):
-        self.conn.mav.command_long_send(
+    ctrl.set_param('ARMING_CHECK', 0)
-            self.conn.target_system,
+    ctrl.set_param('SCHED_LOOP_RATE', 200)
-            self.conn.target_component,
+    ctrl.set_param('FS_THR_ENABLE', 0)
-            mavlink.MAV_CMD_DO_CHANGE_SPEED,
+    ctrl.set_param('FS_GCS_ENABLE', 0)
-            0, 1, speed, -1, 0, 0, 0, 0)
+    sleep(2)
+
+def wait_for_landing(ctrl: Controller, timeout: float = 60.0):
+    print("[UAV] Waiting for landing ...")
+    t0 = time.time()
+    while time.time() - t0 < timeout:
+        ctrl.update_state()
+        elapsed = int(time.time() - t0)
+        print(f"\r[UAV] Descending: {ctrl.altitude:.1f}m  ({elapsed}s)  ",
+              end='', flush=True)
+        if ctrl.altitude < 0.5 and not ctrl.armed:
+            break
+        if ctrl.altitude < 0.3:
+            break
+        sleep(0.5)
+    print(f"\n[UAV] Landed! (alt: {ctrl.altitude:.2f}m)")

src/control/ugv_controller.py

@@ -1,9 +1,4 @@
 #!/usr/bin/env python3
-"""UGV controller using Gazebo transport (gz.transport13).
-
-Publishes Twist to /ugv/cmd_vel, subscribes to /ugv/odometry for
-position feedback. Drives a proportional turn-then-drive controller.
-"""
 
 import math
 import time
@@ -14,13 +9,11 @@ from gz.transport13 import Node
 from gz.msgs10.twist_pb2 import Twist
 from gz.msgs10.odometry_pb2 import Odometry
 
-
 MAX_LINEAR_VEL = 1.0
 MAX_ANGULAR_VEL = 2.0
 POSITION_TOL = 0.5
 YAW_TOL = 0.15
 
-
 class UGVController:
     def __init__(self, cmd_topic="/ugv/cmd_vel", odom_topic="/ugv/odometry",
                  min_speed=0.5, max_speed=1.0):
@@ -69,14 +62,8 @@ class UGVController:
     def stop(self):
         self.send_cmd_vel(0.0, 0.0)
 
-    def distance_to(self, x, y):
-        pos = self.get_position()
-        dx = x - pos['x']
-        dy = y - pos['y']
-        return math.sqrt(dx**2 + dy**2)
-
     def drive_to(self, target_x, target_y, callback=None):
-        """Start driving to (target_x, target_y) in a background thread."""
+
         if self._driving:
             print("[UGV] Already driving, ignoring new target")
             return
@@ -147,6 +134,3 @@ class UGVController:
     def is_driving(self):
         return self._driving
 
-    @property
-    def has_arrived(self):
-        return not self._driving and self._drive_thread is not None

src/main.py

@@ -10,63 +10,19 @@ from pathlib import Path
 
 sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
 
-from control.uav_controller import Controller
+from control.uav_controller import Controller, setup_ardupilot, wait_for_landing
-from control.search import Search
+from navigation.search import Search
 from control.ugv_controller import UGVController
 from vision.object_detector import ObjectDetector
 from vision.camera_processor import CameraProcessor
 from navigation.flight_tracker import FlightTracker
 from utils.recorder import RunRecorder
-
+from utils.convert import ned_to_gz, gz_to_ned, feet_to_meters, mph_to_ms
+from utils.config import load_config
 PROJECT_DIR = Path(__file__).resolve().parent.parent
 CONFIG_DIR = PROJECT_DIR / "config"
 
 
-# ── Coordinate frame conversion ──────────────────────────────
-# ArduPilot LOCAL_POSITION_NED: X=North, Y=East, Z=Down
-# Gazebo Harmonic (ENU default): X=East, Y=North, Z=Up
-# The axes are swapped in the horizontal plane.
-def ned_to_gz(ned_x, ned_y):
-    """Convert ArduPilot NED (North,East) → Gazebo (East,North)."""
-    return ned_y, ned_x
-
-def gz_to_ned(gz_x, gz_y):
-    """Convert Gazebo (East,North) → ArduPilot NED (North,East)."""
-    return gz_y, gz_x
-
-
-def load_config(name: str) -> dict:
-    path = CONFIG_DIR / name
-    if not path.exists():
-        print(f"[WARN] Config not found: {path}")
-        return {}
-    with open(path, 'r') as f:
-        return yaml.safe_load(f) or {}
-
-
-def setup_ardupilot(ctrl: Controller):
-    ctrl.set_param('ARMING_CHECK', 0)
-    ctrl.set_param('SCHED_LOOP_RATE', 200)
-    ctrl.set_param('FS_THR_ENABLE', 0)
-    ctrl.set_param('FS_GCS_ENABLE', 0)
-    sleep(2)
-
-
-def wait_for_landing(ctrl: Controller, timeout: float = 60.0):
-    print("[UAV] Waiting for landing ...")
-    t0 = time.time()
-    while time.time() - t0 < timeout:
-        ctrl.update_state()
-        elapsed = int(time.time() - t0)
-        print(f"\r[UAV] Descending: {ctrl.altitude:.1f}m  ({elapsed}s)  ",
-              end='', flush=True)
-        if ctrl.altitude < 0.5 and not ctrl.armed:
-            break
-        if ctrl.altitude < 0.3:
-            break
-        sleep(0.5)
-    print(f"\n[UAV] Landed! (alt: {ctrl.altitude:.2f}m)")
-
 
 def main():
     parser = argparse.ArgumentParser(description='UAV-UGV Simulation')
@@ -83,11 +39,10 @@ def main():
         recorder = RunRecorder()
         recorder.start_logging()
 
-    uav_cfg = load_config('uav.yaml')
-    search_cfg = load_config('search.yaml')
-    ugv_cfg = load_config('ugv.yaml')
 
-    from utils.convert import feet_to_meters, mph_to_ms
+    uav_cfg = load_config('uav.yaml', CONFIG_DIR)
+    search_cfg = load_config('search.yaml', CONFIG_DIR)
+    ugv_cfg = load_config('ugv.yaml', CONFIG_DIR)
 
     raw_altitude = args.altitude or search_cfg.get('altitude', 4.0)
     altitude = feet_to_meters(raw_altitude)
@@ -123,7 +78,6 @@ def main():
 
     ctrl = Controller(conn_str)
 
-    # Apply configuration speed bounds
     ugv_min_ms = mph_to_ms(ugv_min_mph)
     ugv_max_ms = mph_to_ms(ugv_max_mph)
     ugv = UGVController(cmd_topic=ugv_cmd_topic, odom_topic=ugv_odom_topic,
@@ -189,7 +143,6 @@ def main():
 
     ctrl.configure_ekf_fast_converge()
 
-    # Configure AP speeds in cm/s from max_mph
     uav_max_ms = mph_to_ms(uav_max_mph)
     uav_max_cms = int(uav_max_ms * 100)
     ctrl.configure_speed_limits(wpnav_speed=uav_max_cms, loit_speed=uav_max_cms)
@@ -219,9 +172,7 @@ def main():
 
     if recorder:
         recorder.start_recording(tracker=tracker, camera=camera)
-        recorder.snapshot_camera("pre_search")
 
-    # --- Wire tracker updates into search loop ---
     original_check = search.check_for_markers
     def tracked_check():
         result = original_check()
@@ -230,11 +181,11 @@ def main():
         tracker.update_uav(pos['x'], pos['y'],
                            altitude=ctrl.altitude,
                            heading=ctrl.current_yaw)
-        # Convert UGV Gazebo odom → NED for the tracker
+
         ugv_pos = ugv.get_position()
         ugv_ned_x, ugv_ned_y = gz_to_ned(ugv_pos['x'], ugv_pos['y'])
         tracker.update_ugv(ugv_ned_x, ugv_ned_y)
-        # Log position data
+
         if recorder:
             recorder.log_position(
                 uav_x=pos['x'], uav_y=pos['y'],
@@ -247,13 +198,10 @@ def main():
     search.check_for_markers = tracked_check
     search._on_waypoint_change = lambda idx: tracker.set_active_waypoint(idx)
 
-    # --- Target found → dispatch UGV ---
+    ugv_target_pos = [None, None]
-    # Shared state: set by callback, read by post-search logic
-    ugv_target_pos = [None, None]  # [x, y] — set when target found
 
     def on_target_found(marker_id, x, y):
-        # x, y are in NED (North, East) from the UAV
+
-        # Convert to Gazebo frame for the UGV
         gz_x, gz_y = ned_to_gz(x, y)
         ugv_target_pos[0] = gz_x
         ugv_target_pos[1] = gz_y
@@ -261,20 +209,14 @@ def main():
         print(f"[MAIN] Dispatching UGV to Gazebo({gz_x:.1f}, {gz_y:.1f}) ...")
         if recorder:
             recorder.set_phase("ugv_dispatch")
-            recorder.snapshot_camera("target_found")
-            recorder.snapshot_tracker("target_found")
         ugv.drive_to(gz_x, gz_y)
         search.stop()
 
     search.on_target_found = on_target_found
 
-    # --- Run search (drone searches for target tag ID 1) ---
     results = search.run()
     search_results = search.get_results()
 
-    if recorder and results:
-        recorder.snapshot_camera("marker_detected")
-
     print(f"\n{'=' * 50}")
     print(f"  SEARCH COMPLETE")
     print(f"{'=' * 50}")
@@ -286,7 +228,6 @@ def main():
               f"distance:{info['distance']:.2f}m")
     print(f"{'=' * 50}\n")
 
-    # --- Wait for UGV to arrive, THEN land on it ---
     if ugv_target_pos[0] is not None:
         print("[MAIN] Waiting for UGV to arrive at target ...")
         if recorder:
@@ -302,7 +243,6 @@ def main():
             tracker.update_ugv(ugv_ned_x, ugv_ned_y)
             sleep(0.5)
 
-        # UGV has arrived — fly to above UGV, then land
         ugv_pos = ugv.get_position()
         ugv_ned_x, ugv_ned_y = gz_to_ned(ugv_pos['x'], ugv_pos['y'])
         print(f"\n[MAIN] UGV arrived at Gazebo({ugv_pos['x']:.1f}, {ugv_pos['y']:.1f})")
@@ -312,7 +252,7 @@ def main():
             recorder.set_phase("fly_to_ugv")
 
         ctrl.move_local_ned(ugv_ned_x, ugv_ned_y, -altitude)
-        # Wait until UAV is roughly above UGV
+
         for _ in range(60):
             ctrl.update_state()
             pos = ctrl.get_local_position()
@@ -333,7 +273,6 @@ def main():
         search.landing_enabled = True
         search.running = True
 
-        # Attempt visual servoing landing
         for _ in range(20):
             search.check_for_markers()
             if search.landed:
@@ -344,7 +283,7 @@ def main():
             ctrl.land()
         wait_for_landing(ctrl)
     else:
-        # No target found, just land in place
+
         if recorder:
             recorder.set_phase("landing")
         if not search_results.get('landed'):
@@ -355,8 +294,6 @@ def main():
 
     if recorder:
         recorder.set_phase("complete")
-        recorder.snapshot_camera("post_landing")
-        recorder.snapshot_tracker("final")
         ugv_target_info = None
         if ugv_target_pos[0] is not None:
             ugv_target_info = {
@@ -394,6 +331,5 @@ def main():
         tracker.stop()
         print("[MAIN] Done.")
 
-
 if __name__ == '__main__':
     main()

src/navigation/__init__.py

@@ -1 +1 @@
-"""Navigation module for local path planning and waypoint following."""
+

src/navigation/flight_tracker.py

@@ -10,7 +10,7 @@ import cv2
 from collections import deque
 
 sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
-
+from safety.geofence import point_in_polygon, distance_to_polygon_edge
 
 class FlightTracker:
     BG_COLOR = (30, 30, 30)
@@ -101,36 +101,6 @@ class FlightTracker:
         with self._lock:
             self.active_waypoint = idx
 
-    def _point_in_polygon(self, px, py, polygon):
-        n = len(polygon)
-        inside = False
-        j = n - 1
-        for i in range(n):
-            xi, yi = polygon[i]
-            xj, yj = polygon[j]
-            if ((yi > py) != (yj > py)) and (px < (xj - xi) * (py - yi) / (yj - yi) + xi):
-                inside = not inside
-            j = i
-        return inside
-
-    def _distance_to_polygon_edge(self, px, py, polygon):
-        min_dist = float('inf')
-        n = len(polygon)
-        for i in range(n):
-            x1, y1 = polygon[i]
-            x2, y2 = polygon[(i + 1) % n]
-            dx, dy = x2 - x1, y2 - y1
-            length_sq = dx * dx + dy * dy
-            if length_sq == 0:
-                dist = math.sqrt((px - x1) ** 2 + (py - y1) ** 2)
-            else:
-                t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
-                proj_x = x1 + t * dx
-                proj_y = y1 + t * dy
-                dist = math.sqrt((px - proj_x) ** 2 + (py - proj_y) ** 2)
-            min_dist = min(min_dist, dist)
-        return min_dist
-
     def draw(self):
         frame = np.full((self.window_size, self.total_width, 3),
                         self.BG_COLOR, dtype=np.uint8)
@@ -331,9 +301,9 @@ class FlightTracker:
             y += 30
 
         if self.fence_points:
-            inside = self._point_in_polygon(uav[0], uav[1], self.fence_points)
+            inside = point_in_polygon(uav[0], uav[1], self.fence_points)
             alt_ok = self.fence_alt_min <= alt <= self.fence_alt_max
-            fence_dist = self._distance_to_polygon_edge(uav[0], uav[1], self.fence_points)
+            fence_dist = distance_to_polygon_edge(uav[0], uav[1], self.fence_points)
             if not inside or not alt_ok:
                 status, status_color = "BREACHED", self.FENCE_BREACH_COLOR
             elif fence_dist < self.fence_warn_dist:

src/navigation/local_planner.py

@@ -1,149 +0,0 @@
-#!/usr/bin/env python3
-"""Local Planner - Path planning using relative coordinates only."""
-
-import rclpy
-from rclpy.node import Node
-from geometry_msgs.msg import PoseStamped, Twist, Point
-from nav_msgs.msg import Path, Odometry
-from visualization_msgs.msg import Marker
-import numpy as np
-
-
-class LocalPlanner(Node):
-    
-    def __init__(self):
-        super().__init__('local_planner')
-        
-        self.declare_parameter('max_velocity', 2.0)
-        self.declare_parameter('max_acceleration', 1.0)
-        self.declare_parameter('lookahead_distance', 1.0)
-        self.declare_parameter('goal_tolerance', 0.3)
-        
-        self.max_velocity = self.get_parameter('max_velocity').value
-        self.max_acceleration = self.get_parameter('max_acceleration').value
-        self.lookahead_distance = self.get_parameter('lookahead_distance').value
-        self.goal_tolerance = self.get_parameter('goal_tolerance').value
-        
-        self.current_pose = None
-        self.current_velocity = np.zeros(3)
-        self.path = []
-        self.current_waypoint_idx = 0
-        
-        self.odom_sub = self.create_subscription(
-            Odometry, '/uav/local_position/odom', self.odom_callback, 10)
-        
-        self.goal_sub = self.create_subscription(
-            PoseStamped, '/uav/goal_pose', self.goal_callback, 10)
-        
-        self.path_sub = self.create_subscription(
-            Path, '/uav/planned_path', self.path_callback, 10)
-        
-        self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel', 10)
-        self.setpoint_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10)
-        self.path_viz_pub = self.create_publisher(Path, '/uav/local_path', 10)
-        
-        self.timer = self.create_timer(0.05, self.control_loop)
-        
-        self.get_logger().info('Local Planner Started - GPS-denied navigation')
-    
-    def odom_callback(self, msg):
-        self.current_pose = msg.pose.pose
-        self.current_velocity = np.array([
-            msg.twist.twist.linear.x,
-            msg.twist.twist.linear.y,
-            msg.twist.twist.linear.z
-        ])
-    
-    def goal_callback(self, msg):
-        goal = np.array([
-            msg.pose.position.x,
-            msg.pose.position.y,
-            msg.pose.position.z
-        ])
-        self.path = [goal]
-        self.current_waypoint_idx = 0
-        self.get_logger().info(f'New goal received: [{goal[0]:.2f}, {goal[1]:.2f}, {goal[2]:.2f}]')
-    
-    def path_callback(self, msg):
-        self.path = []
-        for pose_stamped in msg.poses:
-            waypoint = np.array([
-                pose_stamped.pose.position.x,
-                pose_stamped.pose.position.y,
-                pose_stamped.pose.position.z
-            ])
-            self.path.append(waypoint)
-        self.current_waypoint_idx = 0
-        self.get_logger().info(f'Path received with {len(self.path)} waypoints')
-    
-    def control_loop(self):
-        if self.current_pose is None or len(self.path) == 0:
-            return
-        
-        if self.current_waypoint_idx >= len(self.path):
-            self.stop()
-            return
-        
-        current_pos = np.array([
-            self.current_pose.position.x,
-            self.current_pose.position.y,
-            self.current_pose.position.z
-        ])
-        
-        target = self.path[self.current_waypoint_idx]
-        distance = np.linalg.norm(target - current_pos)
-        
-        if distance < self.goal_tolerance:
-            self.current_waypoint_idx += 1
-            if self.current_waypoint_idx >= len(self.path):
-                self.get_logger().info('Path complete!')
-                self.stop()
-                return
-            target = self.path[self.current_waypoint_idx]
-        
-        direction = target - current_pos
-        distance = np.linalg.norm(direction)
-        
-        if distance > 0:
-            direction = direction / distance
-        
-        speed = min(self.max_velocity, distance * 0.5)
-        velocity = direction * speed
-        
-        self.publish_command(velocity, target)
-    
-    def publish_command(self, velocity, target):
-        cmd = Twist()
-        cmd.linear.x = float(velocity[0])
-        cmd.linear.y = float(velocity[1])
-        cmd.linear.z = float(velocity[2])
-        self.cmd_vel_pub.publish(cmd)
-        
-        setpoint = PoseStamped()
-        setpoint.header.stamp = self.get_clock().now().to_msg()
-        setpoint.header.frame_id = 'odom'
-        setpoint.pose.position.x = float(target[0])
-        setpoint.pose.position.y = float(target[1])
-        setpoint.pose.position.z = float(target[2])
-        setpoint.pose.orientation.w = 1.0
-        self.setpoint_pub.publish(setpoint)
-    
-    def stop(self):
-        cmd = Twist()
-        self.cmd_vel_pub.publish(cmd)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = LocalPlanner()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

src/navigation/obstacle_avoidance.py

@@ -1,160 +0,0 @@
-#!/usr/bin/env python3
-"""Obstacle Avoidance - Vision-based obstacle detection and avoidance."""
-
-import rclpy
-from rclpy.node import Node
-from sensor_msgs.msg import Image, LaserScan
-from geometry_msgs.msg import Twist, TwistStamped, Vector3
-from std_msgs.msg import Bool
-from cv_bridge import CvBridge
-import cv2
-import numpy as np
-
-
-class ObstacleAvoidance(Node):
-    
-    def __init__(self):
-        super().__init__('obstacle_avoidance')
-        
-        self.bridge = CvBridge()
-        
-        self.declare_parameter('detection_range', 5.0)
-        self.declare_parameter('safety_margin', 1.0)
-        self.declare_parameter('avoidance_gain', 1.0)
-        self.declare_parameter('enable', True)
-        
-        self.detection_range = self.get_parameter('detection_range').value
-        self.safety_margin = self.get_parameter('safety_margin').value
-        self.avoidance_gain = self.get_parameter('avoidance_gain').value
-        self.enabled = self.get_parameter('enable').value
-        
-        self.obstacle_detected = False
-        self.obstacle_direction = np.zeros(3)
-        self.obstacle_distance = float('inf')
-        
-        self.depth_sub = self.create_subscription(
-            Image, '/uav/camera/depth/image_raw', self.depth_callback, 10)
-        
-        self.scan_sub = self.create_subscription(
-            LaserScan, '/uav/scan', self.scan_callback, 10)
-        
-        self.cmd_vel_sub = self.create_subscription(
-            Twist, '/uav/cmd_vel', self.cmd_vel_callback, 10)
-        
-        self.enable_sub = self.create_subscription(
-            Bool, '/obstacle_avoidance/enable', self.enable_callback, 10)
-        
-        self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel_safe', 10)
-        self.obstacle_pub = self.create_publisher(Bool, '/obstacle_avoidance/obstacle_detected', 10)
-        self.avoidance_vec_pub = self.create_publisher(Vector3, '/obstacle_avoidance/avoidance_vector', 10)
-        
-        self.current_cmd = Twist()
-        
-        self.timer = self.create_timer(0.05, self.avoidance_loop)
-        
-        self.get_logger().info('Obstacle Avoidance Node Started')
-    
-    def enable_callback(self, msg):
-        self.enabled = msg.data
-    
-    def depth_callback(self, msg):
-        try:
-            depth_image = self.bridge.imgmsg_to_cv2(msg, 'passthrough')
-            
-            height, width = depth_image.shape
-            center_region = depth_image[height//3:2*height//3, width//3:2*width//3]
-            
-            valid_depths = center_region[center_region > 0]
-            if len(valid_depths) > 0:
-                min_distance = np.min(valid_depths)
-                
-                if min_distance < self.detection_range:
-                    self.obstacle_detected = True
-                    self.obstacle_distance = min_distance
-                    
-                    left_region = depth_image[:, :width//3]
-                    right_region = depth_image[:, 2*width//3:]
-                    
-                    left_clear = np.mean(left_region[left_region > 0]) if np.any(left_region > 0) else float('inf')
-                    right_clear = np.mean(right_region[right_region > 0]) if np.any(right_region > 0) else float('inf')
-                    
-                    if left_clear > right_clear:
-                        self.obstacle_direction = np.array([0.0, 1.0, 0.0])
-                    else:
-                        self.obstacle_direction = np.array([0.0, -1.0, 0.0])
-                else:
-                    self.obstacle_detected = False
-                    self.obstacle_distance = float('inf')
-                    
-        except Exception as e:
-            self.get_logger().error(f'Depth processing error: {e}')
-    
-    def scan_callback(self, msg):
-        ranges = np.array(msg.ranges)
-        valid_ranges = ranges[np.isfinite(ranges)]
-        
-        if len(valid_ranges) > 0:
-            min_range = np.min(valid_ranges)
-            min_idx = np.argmin(ranges)
-            
-            if min_range < self.detection_range:
-                self.obstacle_detected = True
-                self.obstacle_distance = min_range
-                
-                angle = msg.angle_min + min_idx * msg.angle_increment
-                self.obstacle_direction = np.array([
-                    -np.cos(angle),
-                    -np.sin(angle),
-                    0.0
-                ])
-            else:
-                self.obstacle_detected = False
-    
-    def cmd_vel_callback(self, msg):
-        self.current_cmd = msg
-    
-    def avoidance_loop(self):
-        obstacle_msg = Bool()
-        obstacle_msg.data = self.obstacle_detected
-        self.obstacle_pub.publish(obstacle_msg)
-        
-        if not self.enabled:
-            self.cmd_vel_pub.publish(self.current_cmd)
-            return
-        
-        safe_cmd = Twist()
-        
-        if self.obstacle_detected and self.obstacle_distance < self.safety_margin:
-            repulsion = self.avoidance_gain * (self.safety_margin / self.obstacle_distance)
-            avoidance = self.obstacle_direction * repulsion
-            
-            safe_cmd.linear.x = self.current_cmd.linear.x + float(avoidance[0])
-            safe_cmd.linear.y = self.current_cmd.linear.y + float(avoidance[1])
-            safe_cmd.linear.z = self.current_cmd.linear.z + float(avoidance[2])
-            safe_cmd.angular = self.current_cmd.angular
-            
-            vec_msg = Vector3()
-            vec_msg.x = float(avoidance[0])
-            vec_msg.y = float(avoidance[1])
-            vec_msg.z = float(avoidance[2])
-            self.avoidance_vec_pub.publish(vec_msg)
-        else:
-            safe_cmd = self.current_cmd
-        
-        self.cmd_vel_pub.publish(safe_cmd)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = ObstacleAvoidance()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

src/navigation/patterns/lawnmower.py

@@ -0,0 +1,21 @@
+def plan_lawnmower(start, width, height, lane_spacing):
+    waypoints = [start]
+    sx, sy = start
+    num_lanes = max(1, int(width / lane_spacing) + 1)
+
+    start_x = sx - width / 2.0
+    start_y = sy - height / 2.0
+
+    for lane in range(num_lanes):
+        lane_x = start_x + lane * lane_spacing
+        if lane % 2 == 0:
+            current_y = start_y
+            target_y = start_y + height
+        else:
+            current_y = start_y + height
+            target_y = start_y
+
+        waypoints.append((lane_x, current_y))
+        waypoints.append((lane_x, target_y))
+
+    return waypoints

src/navigation/patterns/levy.py

@@ -0,0 +1,42 @@
+import math
+from scipy.stats import levy, uniform
+
+def plan_levy(start, max_steps, field_size, step_size, angle_dist=None):
+    if angle_dist is None:
+        angle_dist = uniform(loc=-180, scale=360)
+
+    waypoints = [start]
+    x, y = start
+
+    half_size = field_size / 2.0
+    min_x, max_x = start[0] - half_size, start[0] + half_size
+    min_y, max_y = start[1] - half_size, start[1] + half_size
+
+    for _ in range(max_steps):
+        angle_deg = angle_dist.rvs()
+        angle_rad = math.radians(angle_deg)
+        raw_distance = levy.rvs(loc=step_size, scale=step_size)
+        distance = max(raw_distance, step_size)
+
+        nx = x + distance * math.cos(angle_rad)
+        ny = y + distance * math.sin(angle_rad)
+
+        width = max_x - min_x
+        height = max_y - min_y
+
+        if width > 0:
+            nx_shifted = (nx - min_x) % (2 * width)
+            if nx_shifted > width:
+                nx_shifted = 2 * width - nx_shifted
+            nx = min_x + nx_shifted
+
+        if height > 0:
+            ny_shifted = (ny - min_y) % (2 * height)
+            if ny_shifted > height:
+                ny_shifted = 2 * height - ny_shifted
+            ny = min_y + ny_shifted
+
+        x, y = nx, ny
+        waypoints.append((x, y))
+
+    return waypoints

src/navigation/patterns/spiral.py

@@ -0,0 +1,28 @@
+def plan_spiral(start, initial_leg, leg_increment, max_legs):
+    waypoints = [start]
+    x, y = start
+
+    directions = [
+        (0, 1),
+        (1, 0),
+        (0, -1),
+        (-1, 0),
+    ]
+
+    distance = initial_leg
+    dir_idx = 0
+    legs_at_this_dist = 0
+
+    for _ in range(max_legs):
+        dx_dir, dy_dir = directions[dir_idx % 4]
+        x += dx_dir * distance
+        y += dy_dir * distance
+        waypoints.append((x, y))
+
+        dir_idx += 1
+        legs_at_this_dist += 1
+        if legs_at_this_dist >= 2:
+            legs_at_this_dist = 0
+            distance += leg_increment
+
+    return waypoints

src/navigation/search.py

@@ -10,7 +10,10 @@ from enum import Enum
 from control.uav_controller import Controller
 from vision.object_detector import ObjectDetector
 from utils.helpers import distance_2d
-
+from safety.geofence import clip_to_geofence
+from navigation.patterns.spiral import plan_spiral
+from navigation.patterns.lawnmower import plan_lawnmower
+from navigation.patterns.levy import plan_levy
 
 class SearchMode(Enum):
     SPIRAL = "spiral"
@@ -41,12 +44,13 @@ class Search:
         self.on_target_found = None
         self._dispatched_targets = set()
 
-        hfov = 1.3962634  # 80 deg horizontal FOV
+        hfov = 1.3962634
         self.cam_fov_meters = 2.0 * self.altitude * math.tan(hfov / 2.0)
 
         self.spiral_max_legs = 40
-        self.spiral_initial_leg = self.cam_fov_meters
+        overlap_spacing = self.cam_fov_meters * 0.8
-        self.spiral_leg_increment = self.cam_fov_meters * 0.8
+        self.spiral_initial_leg = overlap_spacing
+        self.spiral_leg_increment = overlap_spacing
 
         self.lawn_width = 30.0
         self.lawn_height = 30.0
@@ -67,17 +71,38 @@ class Search:
                 setattr(self, key, value)
 
     def plan(self, start_pos=(0.0, 0.0), geofence_points=None):
+        if geofence_points:
+            warn_dist = getattr(self, 'geofence_warn_dist', 3.0)
+            xs = [p[0] for p in geofence_points]
+            ys = [p[1] for p in geofence_points]
+            safe_w = (max(xs) - min(xs)) - 2 * warn_dist
+            safe_h = (max(ys) - min(ys)) - 2 * warn_dist
+            if safe_w > 0 and safe_h > 0:
+                self.lawn_width = safe_w
+                self.lawn_height = safe_h
+                self.levy_field_size = min(safe_w, safe_h)
+
+        hfov = 1.3962634
+        self.cam_fov_meters = 2.0 * self.altitude * math.tan(hfov / 2.0)
+        overlap_spacing = self.cam_fov_meters * 0.8
+        self.spiral_initial_leg = overlap_spacing
+        self.spiral_leg_increment = overlap_spacing
+        self.lawn_lane_spacing = overlap_spacing
+        self.lawn_lanes = int(self.lawn_width / max(self.lawn_lane_spacing, 0.1)) + 1
+
         if self.mode == SearchMode.SPIRAL:
-            waypoints = self._plan_spiral(start_pos)
+            waypoints = plan_spiral(start_pos, self.spiral_initial_leg, self.spiral_leg_increment, self.spiral_max_legs)
         elif self.mode == SearchMode.LAWNMOWER:
-            waypoints = self._plan_lawnmower(start_pos)
+            waypoints = plan_lawnmower(start_pos, self.lawn_width, self.lawn_height, self.lawn_lane_spacing)
         elif self.mode == SearchMode.LEVY:
-            waypoints = self._plan_levy(start_pos)
+            waypoints = plan_levy(start_pos, self.levy_max_steps, self.levy_field_size, overlap_spacing, self.levy_angle_dist)
         else:
             waypoints = [start_pos]
 
         if geofence_points:
-            waypoints = self._clip_to_geofence(waypoints, geofence_points)
+            warn_dist = getattr(self, 'geofence_warn_dist', 3.0)
+            stop_on_leave = (self.mode == SearchMode.SPIRAL)
+            waypoints = clip_to_geofence(waypoints, geofence_points, warn_dist, stop_on_leave)
 
         self.waypoints = waypoints
         self.current_wp = 0
@@ -91,136 +116,6 @@ class Search:
 
         return waypoints
 
-    def _plan_spiral(self, start):
-        waypoints = [start]
-        x, y = start
-
-        directions = [
-            (0, 1),   # East  (+Y)
-            (1, 0),   # North (+X)
-            (0, -1),  # West  (-Y)
-            (-1, 0),  # South (-X)
-        ]
-
-        distance = self.spiral_initial_leg
-        dir_idx = 0
-        legs_at_this_dist = 0
-
-        for _ in range(self.spiral_max_legs):
-            dx_dir, dy_dir = directions[dir_idx % 4]
-            x += dx_dir * distance
-            y += dy_dir * distance
-            waypoints.append((x, y))
-
-            dir_idx += 1
-            legs_at_this_dist += 1
-            if legs_at_this_dist >= 2:
-                legs_at_this_dist = 0
-                distance += self.spiral_leg_increment
-
-        return waypoints
-
-    def _plan_lawnmower(self, start):
-        waypoints = [start]
-        sx, sy = start
-        lane_spacing = self.lawn_lane_spacing
-        height = self.lawn_height
-        num_lanes = max(1, int(self.lawn_width / lane_spacing))
-
-        for lane in range(num_lanes):
-            lane_x = sx + lane * lane_spacing
-            if lane % 2 == 0:
-                target_y = sy + height
-            else:
-                target_y = sy
-
-            waypoints.append((lane_x, target_y))
-
-            if lane < num_lanes - 1:
-                next_x = sx + (lane + 1) * lane_spacing
-                waypoints.append((next_x, target_y))
-
-        return waypoints
-
-    def _plan_levy(self, start):
-        waypoints = [start]
-        x, y = start
-
-        for _ in range(self.levy_max_steps):
-            angle_deg = self.levy_angle_dist.rvs()
-            angle_rad = math.radians(angle_deg)
-            raw_distance = levy.rvs(loc=1, scale=1)
-            distance = min(max(raw_distance, 1.0), self.levy_field_size)
-
-            x += distance * math.cos(angle_rad)
-            y += distance * math.sin(angle_rad)
-            waypoints.append((x, y))
-
-        return waypoints
-
-    def _clip_to_geofence(self, waypoints, polygon):
-        warn_dist = getattr(self, 'geofence_warn_dist', 3.0)
-        safe_polygon = []
-        if warn_dist > 0:
-            cx = sum(p[0] for p in polygon) / len(polygon)
-            cy = sum(p[1] for p in polygon) / len(polygon)
-            for x, y in polygon:
-                dx = x - cx
-                dy = y - cy
-                length = math.sqrt(dx*dx + dy*dy)
-                if length > 0:
-                    shrink = warn_dist / length
-                    safe_polygon.append((x - dx * shrink, y - dy * shrink))
-                else:
-                    safe_polygon.append((x, y))
-        else:
-            safe_polygon = polygon
-
-        clipped = []
-        for wx, wy in waypoints:
-            if self._point_in_polygon(wx, wy, safe_polygon):
-                clipped.append((wx, wy))
-            else:
-                nearest = self._nearest_point_on_polygon(wx, wy, safe_polygon)
-                if not clipped or distance_2d(clipped[-1], nearest) > 0.5:
-                    clipped.append(nearest)
-        return clipped
-
-    @staticmethod
-    def _point_in_polygon(px, py, polygon):
-        n = len(polygon)
-        inside = False
-        j = n - 1
-        for i in range(n):
-            xi, yi = polygon[i]
-            xj, yj = polygon[j]
-            if ((yi > py) != (yj > py)) and \
-               (px < (xj - xi) * (py - yi) / (yj - yi) + xi):
-                inside = not inside
-            j = i
-        return inside
-
-    @staticmethod
-    def _nearest_point_on_polygon(px, py, polygon):
-        min_dist = float('inf')
-        nearest = (px, py)
-        n = len(polygon)
-        for i in range(n):
-            x1, y1 = polygon[i]
-            x2, y2 = polygon[(i + 1) % n]
-            dx, dy = x2 - x1, y2 - y1
-            length_sq = dx * dx + dy * dy
-            if length_sq == 0:
-                proj = (x1, y1)
-            else:
-                t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
-                proj = (x1 + t * dx, y1 + t * dy)
-            dist = math.sqrt((px - proj[0])**2 + (py - proj[1])**2)
-            if dist < min_dist:
-                min_dist = dist
-                nearest = proj
-        return nearest
-
     def run(self):
         if not self.waypoints:
             self.plan()
@@ -339,7 +234,7 @@ class Search:
 
         IMG_W, IMG_H = 640, 480
         IMG_CX, IMG_CY = IMG_W / 2, IMG_H / 2
-        HFOV = 1.3962634  # 80° horizontal FOV
+        HFOV = 1.3962634
 
         self.ctrl.update_state()
         current_alt = self.ctrl.altitude
@@ -391,7 +286,7 @@ class Search:
 
         IMG_W, IMG_H = 640, 480
         IMG_CX, IMG_CY = IMG_W / 2, IMG_H / 2
-        HFOV = 1.3962634  # 80° horizontal FOV
+        HFOV = 1.3962634
 
         CENTER_PX = 30
         MAX_CORRECTIONS = 200
@@ -467,14 +362,12 @@ class Search:
             ground_w = 2.0 * alt * math.tan(hfov / 2.0)
             m_per_px = ground_w / img_w
 
-            # Calculate movement in the UAV's body frame
             correction_forward = -err_y * m_per_px * gain
             correction_right = err_x * m_per_px * gain
 
             self.ctrl.update_state()
             yaw = self.ctrl.current_yaw
 
-            # Rotate body correction (Forward, Right) into world NED (North, East)
             correction_n = correction_forward * math.cos(yaw) - correction_right * math.sin(yaw)
             correction_e = correction_forward * math.sin(yaw) + correction_right * math.cos(yaw)
 
@@ -496,7 +389,7 @@ class Search:
         dist_total = distance_2d((pos['x'], pos['y']), (target_x, target_y))
 
         if timeout is None:
-            # Dynamic timeout: 30s base time + 3 seconds per meter
+
             timeout = 30.0 + (dist_total * 3.0)
 
         t0 = time.time()
@@ -521,23 +414,6 @@ class Search:
         print()
         return False
 
-    def _wait_arrival(self, target_x, target_y, timeout=15.0):
-        t0 = time.time()
-        while time.time() - t0 < timeout and self.running:
-            self.ctrl.update_state()
-            pos = self.ctrl.get_local_position()
-            dist = distance_2d(
-                (pos['x'], pos['y']),
-                (target_x, target_y)
-            )
-            elapsed = int(time.time() - t0)
-            print(f"\r[SEARCH] Approaching marker: {dist:.1f}m  ({elapsed}s)  ",
-                  end='', flush=True)
-            if dist < self.POSITION_TOLERANCE:
-                print()
-                return True
-            sleep(self.CHECK_INTERVAL)
-        print()
         return False
 
     def move_to_local(self, x, y):

src/navigation/waypoint_follower.py

@@ -1,183 +0,0 @@
-#!/usr/bin/env python3
-"""Waypoint Follower - Follows relative waypoints without GPS."""
-
-import rclpy
-from rclpy.node import Node
-from geometry_msgs.msg import PoseStamped, Twist
-from nav_msgs.msg import Odometry, Path
-from std_msgs.msg import Int32, String, Bool
-import numpy as np
-from enum import Enum
-
-
-class WaypointState(Enum):
-    IDLE = 0
-    NAVIGATING = 1
-    REACHED = 2
-    PAUSED = 3
-
-
-class WaypointFollower(Node):
-    
-    def __init__(self):
-        super().__init__('waypoint_follower')
-        
-        self.declare_parameter('waypoint_radius', 0.5)
-        self.declare_parameter('max_velocity', 2.0)
-        self.declare_parameter('kp_linear', 0.8)
-        self.declare_parameter('kp_angular', 1.0)
-        
-        self.waypoint_radius = self.get_parameter('waypoint_radius').value
-        self.max_velocity = self.get_parameter('max_velocity').value
-        self.kp_linear = self.get_parameter('kp_linear').value
-        self.kp_angular = self.get_parameter('kp_angular').value
-        
-        self.waypoints = []
-        self.current_idx = 0
-        self.state = WaypointState.IDLE
-        self.current_pose = None
-        
-        self.odom_sub = self.create_subscription(
-            Odometry, '/uav/local_position/odom', self.odom_callback, 10)
-        
-        self.waypoint_sub = self.create_subscription(
-            PoseStamped, '/waypoint_follower/add_waypoint', self.add_waypoint_callback, 10)
-        
-        self.path_sub = self.create_subscription(
-            Path, '/waypoint_follower/set_path', self.set_path_callback, 10)
-        
-        self.command_sub = self.create_subscription(
-            String, '/waypoint_follower/command', self.command_callback, 10)
-        
-        self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel', 10)
-        self.current_waypoint_pub = self.create_publisher(Int32, '/waypoint_follower/current_index', 10)
-        self.status_pub = self.create_publisher(String, '/waypoint_follower/status', 10)
-        self.reached_pub = self.create_publisher(Bool, '/waypoint_follower/waypoint_reached', 10)
-        
-        self.timer = self.create_timer(0.05, self.control_loop)
-        
-        self.get_logger().info('Waypoint Follower Started - Using LOCAL coordinates only')
-    
-    def odom_callback(self, msg):
-        self.current_pose = msg.pose.pose
-    
-    def add_waypoint_callback(self, msg):
-        waypoint = np.array([
-            msg.pose.position.x,
-            msg.pose.position.y,
-            msg.pose.position.z
-        ])
-        self.waypoints.append(waypoint)
-        self.get_logger().info(f'Added waypoint {len(self.waypoints)}: {waypoint}')
-    
-    def set_path_callback(self, msg):
-        self.waypoints = []
-        for pose_stamped in msg.poses:
-            waypoint = np.array([
-                pose_stamped.pose.position.x,
-                pose_stamped.pose.position.y,
-                pose_stamped.pose.position.z
-            ])
-            self.waypoints.append(waypoint)
-        self.current_idx = 0
-        self.get_logger().info(f'Set path with {len(self.waypoints)} waypoints')
-    
-    def command_callback(self, msg):
-        cmd = msg.data.lower()
-        
-        if cmd == 'start':
-            if len(self.waypoints) > 0:
-                self.state = WaypointState.NAVIGATING
-                self.get_logger().info('Starting waypoint navigation')
-        elif cmd == 'pause':
-            self.state = WaypointState.PAUSED
-            self.stop()
-        elif cmd == 'resume':
-            self.state = WaypointState.NAVIGATING
-        elif cmd == 'stop':
-            self.state = WaypointState.IDLE
-            self.stop()
-        elif cmd == 'clear':
-            self.waypoints = []
-            self.current_idx = 0
-            self.state = WaypointState.IDLE
-        elif cmd == 'next':
-            if self.current_idx < len(self.waypoints) - 1:
-                self.current_idx += 1
-        elif cmd == 'prev':
-            if self.current_idx > 0:
-                self.current_idx -= 1
-    
-    def control_loop(self):
-        self.publish_status()
-        
-        if self.state != WaypointState.NAVIGATING:
-            return
-        
-        if self.current_pose is None or len(self.waypoints) == 0:
-            return
-        
-        if self.current_idx >= len(self.waypoints):
-            self.state = WaypointState.IDLE
-            self.stop()
-            self.get_logger().info('All waypoints reached!')
-            return
-        
-        current_pos = np.array([
-            self.current_pose.position.x,
-            self.current_pose.position.y,
-            self.current_pose.position.z
-        ])
-        
-        target = self.waypoints[self.current_idx]
-        error = target - current_pos
-        distance = np.linalg.norm(error)
-        
-        if distance < self.waypoint_radius:
-            reached_msg = Bool()
-            reached_msg.data = True
-            self.reached_pub.publish(reached_msg)
-            
-            self.get_logger().info(f'Reached waypoint {self.current_idx + 1}/{len(self.waypoints)}')
-            self.current_idx += 1
-            return
-        
-        direction = error / distance
-        speed = min(self.kp_linear * distance, self.max_velocity)
-        velocity = direction * speed
-        
-        cmd = Twist()
-        cmd.linear.x = float(velocity[0])
-        cmd.linear.y = float(velocity[1])
-        cmd.linear.z = float(velocity[2])
-        
-        self.cmd_vel_pub.publish(cmd)
-    
-    def stop(self):
-        cmd = Twist()
-        self.cmd_vel_pub.publish(cmd)
-    
-    def publish_status(self):
-        idx_msg = Int32()
-        idx_msg.data = self.current_idx
-        self.current_waypoint_pub.publish(idx_msg)
-        
-        status_msg = String()
-        status_msg.data = f'{self.state.name}|{self.current_idx}/{len(self.waypoints)}'
-        self.status_pub.publish(status_msg)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = WaypointFollower()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

src/safety/__init__.py

@@ -1 +1 @@
-"""Safety module for geofencing and failsafe handling."""
+

src/safety/failsafe_handler.py

@@ -1,154 +0,0 @@
-#!/usr/bin/env python3
-"""Failsafe Handler - Emergency procedures for vision loss and other failures."""
-
-import rclpy
-from rclpy.node import Node
-from geometry_msgs.msg import PoseStamped, TwistStamped
-from std_msgs.msg import String, Bool
-from sensor_msgs.msg import BatteryState
-from enum import Enum
-
-
-class FailsafeState(Enum):
-    NORMAL = 0
-    VISION_LOSS = 1
-    LOW_BATTERY = 2
-    GEOFENCE_BREACH = 3
-    COMM_LOSS = 4
-    EMERGENCY = 5
-
-
-class FailsafeHandler(Node):
-    
-    def __init__(self):
-        super().__init__('failsafe_handler')
-        
-        self.declare_parameter('vision_loss_timeout', 5.0)
-        self.declare_parameter('low_battery_threshold', 20.0)
-        self.declare_parameter('critical_battery_threshold', 10.0)
-        self.declare_parameter('action_on_vision_loss', 'HOLD')
-        self.declare_parameter('action_on_low_battery', 'RTL')
-        self.declare_parameter('action_on_critical_battery', 'LAND')
-        
-        self.vision_timeout = self.get_parameter('vision_loss_timeout').value
-        self.low_battery = self.get_parameter('low_battery_threshold').value
-        self.critical_battery = self.get_parameter('critical_battery_threshold').value
-        self.vision_loss_action = self.get_parameter('action_on_vision_loss').value
-        self.low_battery_action = self.get_parameter('action_on_low_battery').value
-        self.critical_battery_action = self.get_parameter('action_on_critical_battery').value
-        
-        self.state = FailsafeState.NORMAL
-        self.last_vo_time = None
-        self.battery_percent = 100.0
-        self.geofence_ok = True
-        
-        self.vision_triggered = False
-        self.battery_triggered = False
-        
-        self.vo_pose_sub = self.create_subscription(
-            PoseStamped, '/uav/visual_odometry/pose', self.vo_callback, 10)
-        
-        self.battery_sub = self.create_subscription(
-            BatteryState, '/uav/battery', self.battery_callback, 10)
-        
-        self.geofence_sub = self.create_subscription(
-            Bool, '/geofence/status', self.geofence_callback, 10)
-        
-        self.geofence_action_sub = self.create_subscription(
-            String, '/geofence/action', self.geofence_action_callback, 10)
-        
-        self.uav_cmd_pub = self.create_publisher(String, '/uav/controller/command', 10)
-        self.ugv_cmd_pub = self.create_publisher(String, '/ugv/controller/command', 10)
-        self.failsafe_status_pub = self.create_publisher(String, '/failsafe/status', 10)
-        self.failsafe_active_pub = self.create_publisher(Bool, '/failsafe/active', 10)
-        
-        self.check_timer = self.create_timer(0.5, self.check_failsafes)
-        
-        self.get_logger().info('Failsafe Handler Started')
-    
-    def vo_callback(self, msg):
-        self.last_vo_time = self.get_clock().now()
-        
-        if self.state == FailsafeState.VISION_LOSS:
-            self.state = FailsafeState.NORMAL
-            self.vision_triggered = False
-            self.get_logger().info('Vision restored - returning to normal')
-    
-    def battery_callback(self, msg):
-        self.battery_percent = msg.percentage * 100.0
-    
-    def geofence_callback(self, msg):
-        self.geofence_ok = msg.data
-    
-    def geofence_action_callback(self, msg):
-        self.state = FailsafeState.GEOFENCE_BREACH
-        self.execute_action(msg.data)
-    
-    def check_failsafes(self):
-        if self.last_vo_time is not None:
-            elapsed = (self.get_clock().now() - self.last_vo_time).nanoseconds / 1e9
-            if elapsed > self.vision_timeout and not self.vision_triggered:
-                self.state = FailsafeState.VISION_LOSS
-                self.vision_triggered = True
-                self.get_logger().error(f'VISION LOSS - No VO for {elapsed:.1f}s')
-                self.execute_action(self.vision_loss_action)
-        
-        if self.battery_percent <= self.critical_battery and not self.battery_triggered:
-            self.state = FailsafeState.LOW_BATTERY
-            self.battery_triggered = True
-            self.get_logger().error(f'CRITICAL BATTERY: {self.battery_percent:.1f}%')
-            self.execute_action(self.critical_battery_action)
-        elif self.battery_percent <= self.low_battery and not self.battery_triggered:
-            self.state = FailsafeState.LOW_BATTERY
-            self.battery_triggered = True
-            self.get_logger().warn(f'LOW BATTERY: {self.battery_percent:.1f}%')
-            self.execute_action(self.low_battery_action)
-        
-        self.publish_status()
-    
-    def execute_action(self, action):
-        cmd_msg = String()
-        
-        if action == 'HOLD':
-            cmd_msg.data = 'hold'
-        elif action == 'RTL':
-            cmd_msg.data = 'rtl'
-        elif action == 'LAND':
-            cmd_msg.data = 'land'
-        elif action == 'STOP':
-            cmd_msg.data = 'stop'
-        else:
-            cmd_msg.data = 'hold'
-        
-        self.uav_cmd_pub.publish(cmd_msg)
-        
-        ugv_cmd = String()
-        ugv_cmd.data = 'stop'
-        self.ugv_cmd_pub.publish(ugv_cmd)
-        
-        self.get_logger().warn(f'Failsafe action executed: {action}')
-    
-    def publish_status(self):
-        status_msg = String()
-        status_msg.data = f'{self.state.name}|battery:{self.battery_percent:.1f}|geofence:{self.geofence_ok}'
-        self.failsafe_status_pub.publish(status_msg)
-        
-        active_msg = Bool()
-        active_msg.data = self.state != FailsafeState.NORMAL
-        self.failsafe_active_pub.publish(active_msg)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = FailsafeHandler()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

src/safety/geofence.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python3
+
+import math
+from utils.helpers import distance_2d
+
+def point_in_polygon(px, py, polygon):
+    n = len(polygon)
+    inside = False
+    j = n - 1
+    for i in range(n):
+        xi, yi = polygon[i]
+        xj, yj = polygon[j]
+        if ((yi > py) != (yj > py)) and (px < (xj - xi) * (py - yi) / (yj - yi) + xi):
+            inside = not inside
+        j = i
+    return inside
+
+def nearest_point_on_polygon(px, py, polygon):
+    min_dist = float('inf')
+    nearest = (px, py)
+    n = len(polygon)
+    for i in range(n):
+        x1, y1 = polygon[i]
+        x2, y2 = polygon[(i + 1) % n]
+        dx, dy = x2 - x1, y2 - y1
+        length_sq = dx * dx + dy * dy
+        if length_sq == 0:
+            proj = (x1, y1)
+        else:
+            t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
+            proj = (x1 + t * dx, y1 + t * dy)
+        dist = math.sqrt((px - proj[0])**2 + (py - proj[1])**2)
+        if dist < min_dist:
+            min_dist = dist
+            nearest = proj
+    return nearest
+
+def distance_to_polygon_edge(px, py, polygon):
+    min_dist = float('inf')
+    n = len(polygon)
+    for i in range(n):
+        x1, y1 = polygon[i]
+        x2, y2 = polygon[(i + 1) % n]
+        dx, dy = x2 - x1, y2 - y1
+        length_sq = dx * dx + dy * dy
+        if length_sq == 0:
+            dist = math.sqrt((px - x1) ** 2 + (py - y1) ** 2)
+        else:
+            t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
+            proj_x = x1 + t * dx
+            proj_y = y1 + t * dy
+            dist = math.sqrt((px - proj_x) ** 2 + (py - proj_y) ** 2)
+        min_dist = min(min_dist, dist)
+    return min_dist
+
+def clip_to_geofence(waypoints, polygon, warn_dist=3.0, stop_on_leave=False):
+    safe_polygon = []
+    if warn_dist > 0:
+        cx = sum(p[0] for p in polygon) / len(polygon)
+        cy = sum(p[1] for p in polygon) / len(polygon)
+        for x, y in polygon:
+            dx = x - cx
+            dy = y - cy
+            length = math.sqrt(dx*dx + dy*dy)
+            if length > 0:
+                shrink = warn_dist / length
+                safe_polygon.append((x - dx * shrink, y - dy * shrink))
+            else:
+                safe_polygon.append((x, y))
+    else:
+        safe_polygon = polygon
+
+    clipped = []
+    consecutive_outside = 0
+    for wx, wy in waypoints:
+        if point_in_polygon(wx, wy, safe_polygon):
+            clipped.append((wx, wy))
+            consecutive_outside = 0
+        else:
+            consecutive_outside += 1
+            if stop_on_leave and consecutive_outside >= 2:
+                break
+            nearest = nearest_point_on_polygon(wx, wy, safe_polygon)
+            if not clipped or distance_2d(clipped[-1], nearest) > 0.5:
+                clipped.append(nearest)
+    return clipped

src/safety/geofence_monitor.py

@@ -1,162 +0,0 @@
-#!/usr/bin/env python3
-"""Geofence Monitor - GPS ONLY used for safety boundaries."""
-
-import rclpy
-from rclpy.node import Node
-from sensor_msgs.msg import NavSatFix
-from geometry_msgs.msg import Point
-from std_msgs.msg import Bool, String
-from visualization_msgs.msg import Marker
-import numpy as np
-from shapely.geometry import Point as ShapelyPoint, Polygon
-
-
-class GeofenceMonitor(Node):
-    
-    def __init__(self):
-        super().__init__('geofence_monitor')
-        
-        self.declare_parameter('fence_enabled', True)
-        self.declare_parameter('fence_type', 'polygon')
-        self.declare_parameter('warning_distance', 10.0)
-        self.declare_parameter('breach_action', 'RTL')
-        self.declare_parameter('min_altitude', 0.0)
-        self.declare_parameter('max_altitude', 50.0)
-        self.declare_parameter('consecutive_breaches_required', 3)
-        
-        self.fence_enabled = self.get_parameter('fence_enabled').value
-        self.fence_type = self.get_parameter('fence_type').value
-        self.warning_distance = self.get_parameter('warning_distance').value
-        self.breach_action = self.get_parameter('breach_action').value
-        self.min_altitude = self.get_parameter('min_altitude').value
-        self.max_altitude = self.get_parameter('max_altitude').value
-        self.breaches_required = self.get_parameter('consecutive_breaches_required').value
-        
-        self.fence_points = [
-            (47.397742, 8.545594),
-            (47.398242, 8.545594),
-            (47.398242, 8.546094),
-            (47.397742, 8.546094),
-        ]
-        self.fence_polygon = Polygon(self.fence_points)
-        
-        self.current_position = None
-        self.inside_fence = True
-        self.breach_count = 0
-        
-        self.gps_sub = self.create_subscription(
-            NavSatFix, '/uav/mavros/global_position/global', self.gps_callback, 10)
-        
-        self.fence_status_pub = self.create_publisher(Bool, '/geofence/status', 10)
-        self.fence_action_pub = self.create_publisher(String, '/geofence/action', 10)
-        self.fence_viz_pub = self.create_publisher(Marker, '/geofence/visualization', 10)
-        self.warning_pub = self.create_publisher(String, '/geofence/warning', 10)
-        
-        self.viz_timer = self.create_timer(1.0, self.publish_visualization)
-        
-        self.get_logger().info('Geofence Monitor Started - GPS ONLY for safety boundaries')
-        self.get_logger().info(f'Fence type: {self.fence_type}, Action: {self.breach_action}')
-    
-    def gps_callback(self, msg):
-        if not self.fence_enabled:
-            return
-        
-        self.current_position = (msg.latitude, msg.longitude, msg.altitude)
-        
-        point = ShapelyPoint(msg.latitude, msg.longitude)
-        inside_horizontal = self.fence_polygon.contains(point)
-        inside_vertical = self.min_altitude <= msg.altitude <= self.max_altitude
-        
-        self.inside_fence = inside_horizontal and inside_vertical
-        
-        status_msg = Bool()
-        status_msg.data = self.inside_fence
-        self.fence_status_pub.publish(status_msg)
-        
-        if not self.inside_fence:
-            self.breach_count += 1
-            
-            if not inside_horizontal:
-                distance = self.fence_polygon.exterior.distance(point) * 111000
-                self.get_logger().warn(f'GEOFENCE: Outside boundary by {distance:.1f}m')
-            
-            if not inside_vertical:
-                if msg.altitude < self.min_altitude:
-                    self.get_logger().warn(f'GEOFENCE: Below min altitude ({msg.altitude:.1f}m)')
-                else:
-                    self.get_logger().warn(f'GEOFENCE: Above max altitude ({msg.altitude:.1f}m)')
-            
-            if self.breach_count >= self.breaches_required:
-                self.trigger_breach_action()
-        else:
-            self.breach_count = 0
-            
-            if inside_horizontal:
-                distance_to_edge = self.fence_polygon.exterior.distance(point) * 111000
-                if distance_to_edge < self.warning_distance:
-                    warning_msg = String()
-                    warning_msg.data = f'Approaching boundary: {distance_to_edge:.1f}m remaining'
-                    self.warning_pub.publish(warning_msg)
-    
-    def trigger_breach_action(self):
-        action_msg = String()
-        action_msg.data = self.breach_action
-        self.fence_action_pub.publish(action_msg)
-        
-        self.get_logger().error(f'GEOFENCE BREACH ACTION: {self.breach_action}')
-        self.breach_count = 0
-    
-    def publish_visualization(self):
-        if not self.fence_enabled:
-            return
-        
-        marker = Marker()
-        marker.header.stamp = self.get_clock().now().to_msg()
-        marker.header.frame_id = 'map'
-        marker.ns = 'geofence'
-        marker.id = 0
-        marker.type = Marker.LINE_STRIP
-        marker.action = Marker.ADD
-        
-        marker.scale.x = 2.0
-        
-        if self.inside_fence:
-            marker.color.r = 0.0
-            marker.color.g = 1.0
-            marker.color.b = 0.0
-        else:
-            marker.color.r = 1.0
-            marker.color.g = 0.0
-            marker.color.b = 0.0
-        marker.color.a = 0.8
-        
-        for lat, lon in self.fence_points:
-            p = Point()
-            p.x = (lon - self.fence_points[0][1]) * 111000
-            p.y = (lat - self.fence_points[0][0]) * 111000
-            p.z = 0.0
-            marker.points.append(p)
-        
-        p = Point()
-        p.x = 0.0
-        p.y = 0.0
-        p.z = 0.0
-        marker.points.append(p)
-        
-        self.fence_viz_pub.publish(marker)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = GeofenceMonitor()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

src/utils/__init__.py

@@ -0,0 +1 @@
+

src/utils/config.py

@@ -0,0 +1,13 @@
+#!/usr/bin/env python3
+
+import yaml
+from pathlib import Path
+
+def load_config(name: str, config_dir: Path) -> dict:
+    path = config_dir / name
+    if not path.exists():
+        print(f"[WARN] Config not found: {path}")
+        return {}
+    with open(path, 'r') as f:
+        return yaml.safe_load(f) or {}
+

src/utils/convert.py

@@ -1,22 +1,13 @@
 #!/usr/bin/env python3
-"""
-Unit conversions: Customary (US) to Metric.
-"""
 
 def feet_to_meters(feet: float) -> float:
     return feet * 0.3048
 
-def inches_to_meters(inches: float) -> float:
-    return inches * 0.0254
-
-def miles_to_meters(miles: float) -> float:
-    return miles * 1609.34
-
-def pounds_to_kg(pounds: float) -> float:
-    return pounds * 0.453592
-
-def ounces_to_kg(ounces: float) -> float:
-    return ounces * 0.0283495
-
 def mph_to_ms(mph: float) -> float:
     return mph * 0.44704
+
+def ned_to_gz(ned_x, ned_y):
+    return ned_y, ned_x
+
+def gz_to_ned(gz_x, gz_y):
+    return gz_y, gz_x

src/utils/helpers.py

@@ -1,97 +1,6 @@
 #!/usr/bin/env python3
 
 import numpy as np
-import yaml
-from pathlib import Path
-
-
-def load_yaml_config(filepath):
-    with open(filepath, 'r') as f:
-        return yaml.safe_load(f)
-
-
-def save_yaml_config(data, filepath):
-    with open(filepath, 'w') as f:
-        yaml.dump(data, f, default_flow_style=False)
-
-
-def clamp(value, min_val, max_val):
-    return max(min_val, min(value, max_val))
-
-
-def lerp(a, b, t):
-    return a + (b - a) * clamp(t, 0.0, 1.0)
-
-
-def smooth_step(edge0, edge1, x):
-    t = clamp((x - edge0) / (edge1 - edge0), 0.0, 1.0)
-    return t * t * (3.0 - 2.0 * t)
-
 
 def distance_2d(p1, p2):
     return np.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2)
-
-
-def distance_3d(p1, p2):
-    return np.linalg.norm(np.array(p2) - np.array(p1))
-
-
-def moving_average(values, window_size):
-    if len(values) < window_size:
-        return np.mean(values)
-    return np.mean(values[-window_size:])
-
-
-class RateLimiter:
-    def __init__(self, max_rate):
-        self.max_rate = max_rate
-        self.last_value = 0.0
-    
-    def apply(self, value, dt):
-        max_change = self.max_rate * dt
-        change = clamp(value - self.last_value, -max_change, max_change)
-        self.last_value += change
-        return self.last_value
-
-
-class LowPassFilter:
-    def __init__(self, alpha=0.1):
-        self.alpha = alpha
-        self.value = None
-    
-    def apply(self, new_value):
-        if self.value is None:
-            self.value = new_value
-        else:
-            self.value = self.alpha * new_value + (1 - self.alpha) * self.value
-        return self.value
-    
-    def reset(self):
-        self.value = None
-
-
-class PIDController:
-    def __init__(self, kp, ki, kd, output_limits=None):
-        self.kp = kp
-        self.ki = ki
-        self.kd = kd
-        self.output_limits = output_limits
-        self.integral = 0.0
-        self.last_error = 0.0
-        self.last_time = None
-    
-    def compute(self, error, current_time):
-        dt = 0.0 if self.last_time is None else current_time - self.last_time
-        self.integral += error * dt
-        derivative = (error - self.last_error) / dt if dt > 0 else 0.0
-        output = self.kp * error + self.ki * self.integral + self.kd * derivative
-        if self.output_limits:
-            output = clamp(output, self.output_limits[0], self.output_limits[1])
-        self.last_error = error
-        self.last_time = current_time
-        return output
-    
-    def reset(self):
-        self.integral = 0.0
-        self.last_error = 0.0
-        self.last_time = None

src/utils/recorder.py

@@ -1,8 +1,4 @@
 #!/usr/bin/env python3
-"""Run recorder — captures simulation video and logs, and uploads to local REST API backend.
-
-Each run gets a temporary folder, and all resources are uploaded directly to the backend.
-"""
 
 import os
 import sys
@@ -18,6 +14,7 @@ import requests
 from pathlib import Path
 from datetime import datetime
 import yaml
+import signal
 
 try:
     import PIL.Image
@@ -36,7 +33,6 @@ class RunRecorder:
         self.search_start = 0.0
         self.search_duration = 0.0
 
-        # Gather config data as JSON
         config_data = {}
         for cfg in ["search.yaml", "ugv.yaml", "uav.yaml"]:
             p = project_dir / "config" / cfg
@@ -63,7 +59,7 @@ class RunRecorder:
             if resp.status_code == 200:
                 data = resp.json()
                 self.sim_id = data.get("id", 0)
-                self.sim_name = data.get("name", f"simulation_{self.sim_id}")
+                self.sim_name = f"simulation_{self.sim_id}"
         except Exception as e:
             print(f"[REC] API create failed: {e}")
 
@@ -105,9 +101,6 @@ class RunRecorder:
         self._record_thread = None
         self._lock = threading.Lock()
 
-        # Gazebo window capture via xwd
-        self._gazebo_wid = None
-        self._find_gazebo_window()
         threading.Thread(target=self._upload_hardware_info, daemon=True).start()
 
     def _upload_hardware_info(self):
@@ -160,23 +153,41 @@ class RunRecorder:
         except Exception as e:
             print(f"[REC] Upload failed for {filename}: {e}")
 
-    def _find_gazebo_window(self):
+    def _find_gazebo_window_ffmpeg(self):
         try:
             cmd = "xwininfo -root -tree | grep -i 'gazebo\|gz sim'"
             output = subprocess.check_output(cmd, shell=True).decode('utf-8')
-            lines = output.strip().split('\n')
+            lines = [l for l in output.strip().split('\n') if "1x1" not in l and "Gazebo Sim" in l]
             if not lines:
-                print("[REC] Gazebo window not found")
+                return None
-                return
 
             wid_line = lines[0]
             wid_match = re.search(r'(0x[0-9a-fA-F]+)', wid_line)
-            if not wid_match:
+            if wid_match:
-                return
+                return wid_match.group(1)
-            self._gazebo_wid = wid_match.group(1)
+        except Exception:
-            print(f"[REC] Found Gazebo window ID: {self._gazebo_wid}")
+            pass
-        except Exception as e:
+        return None
-            print(f"[REC] Error finding Gazebo window: {e}")
+
+    def _get_window_geometry(self, window_id):
+        try:
+            geo = subprocess.run(
+                ["xdotool", "getwindowgeometry", window_id],
+                capture_output=True, text=True
+            )
+            x = y = width = height = None
+            for line in geo.stdout.splitlines():
+                if "Position:" in line:
+                    pos = line.split("Position:")[1].strip().split()[0]
+                    x, y = map(int, pos.split(","))
+                elif "Geometry:" in line:
+                    size = line.split("Geometry:")[1].strip()
+                    width, height = map(int, size.split("x"))
+            if None in (x, y, width, height):
+                return None
+            return x, y, width, height
+        except FileNotFoundError:
+            return None
 
     def set_phase(self, phase: str):
         if phase == "takeoff":
@@ -202,8 +213,56 @@ class RunRecorder:
         self._camera_ref = camera
         self._recording = True
 
-        if not self._gazebo_wid:
+        window_id = self._find_gazebo_window_ffmpeg()
-             self._find_gazebo_window()
+        geo = None
+        if window_id:
+            geo = self._get_window_geometry(window_id)
+
+        if geo:
+            x, y, width, height = geo
+            width += width % 2
+            height += height % 2
+        else:
+            x, y, width, height = 0, 0, 1920, 1080
+
+        self.gazebo_output_file = str(self.run_dir / "gazebo.mp4")
+        display = os.environ.get("DISPLAY", ":0")
+        if not display.endswith(".0"):
+            display += ".0"
+
+        cmd = [
+            "ffmpeg", "-y",
+            "-f", "x11grab"
+        ]
+
+        if window_id:
+            cmd.extend([
+                "-window_id", str(window_id),
+                "-r", str(self.fps),
+                "-i", display,
+            ])
+        else:
+            cmd.extend([
+                "-r", str(self.fps),
+                "-s", f"{width}x{height}",
+                "-i", f"{display}+{x},{y}",
+            ])
+
+        cmd.extend([
+            "-c:v", "libx264",
+            "-preset", "ultrafast",
+            "-crf", "18",
+            "-pix_fmt", "yuv420p",
+            "-vf", "pad=ceil(iw/2)*2:ceil(ih/2)*2",
+            "-movflags", "+faststart",
+            self.gazebo_output_file
+        ])
+        try:
+            self._gazebo_ffmpeg_proc = subprocess.Popen(cmd, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
+            print(f"[REC] Started recording Gazebo window via ffmpeg to gazebo.mp4 (ID: {window_id})")
+        except Exception as e:
+            print(f"[REC] Failed to start ffmpeg: {e}")
+            self._gazebo_ffmpeg_proc = None
 
         self._record_thread = threading.Thread(target=self._record_loop, daemon=True)
         self._record_thread.start()
@@ -231,26 +290,6 @@ class RunRecorder:
                 except Exception:
                     pass
 
-            if self._gazebo_wid and HAS_PIL:
-                try:
-                    proc = subprocess.run(
-                        ['xwd', '-id', self._gazebo_wid, '-out', '/dev/stdout'],
-                        stdout=subprocess.PIPE,
-                        stderr=subprocess.PIPE
-                    )
-                    if proc.returncode == 0:
-                        img_pil = PIL.Image.open(io.BytesIO(proc.stdout))
-                        img_np = np.array(img_pil)
-                        if img_pil.mode == 'RGB':
-                            img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGB2BGR)
-                        elif img_pil.mode == 'RGBA':
-                            img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGBA2BGR)
-                        else:
-                            img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGB2BGR)
-                        self._write_gazebo_frame(img_bgr)
-                except Exception:
-                    pass
-
             elapsed = time.time() - t0
             sleep_time = max(0, interval - elapsed)
             time.sleep(sleep_time)
@@ -275,23 +314,6 @@ class RunRecorder:
         self._camera_writer.write(frame)
         self._camera_frames += 1
 
-    def _write_gazebo_frame(self, frame):
-        h, w = frame.shape[:2]
-        if w % 2 != 0: w -= 1
-        if h % 2 != 0: h -= 1
-        frame = frame[:h, :w]
-        
-        if self._gazebo_writer is None:
-            self._gazebo_size = (w, h)
-            fourcc = cv2.VideoWriter_fourcc(*'XVID')
-            path = str(self.run_dir / "gazebo.avi")
-            self._gazebo_writer = cv2.VideoWriter(path, fourcc, self.fps, (w, h))
-        elif (w, h) != self._gazebo_size:
-             frame = cv2.resize(frame, self._gazebo_size)
-            
-        self._gazebo_writer.write(frame)
-        self._gazebo_frames += 1
-
     def snapshot_camera(self, label="snapshot"):
         if self._camera_ref is None:
             return
@@ -322,6 +344,14 @@ class RunRecorder:
         if self._record_thread:
             self._record_thread.join(timeout=3.0)
 
+        if hasattr(self, '_gazebo_ffmpeg_proc') and self._gazebo_ffmpeg_proc:
+            try:
+                self._gazebo_ffmpeg_proc.send_signal(signal.SIGINT)
+                self._gazebo_ffmpeg_proc.wait(timeout=5)
+            except Exception:
+                pass
+            self._upload_file(self.gazebo_output_file, "gazebo.mp4")
+
         if self._last_tracker_frame is not None:
             filename = "flight_path.png"
             path = self.run_dir / filename
@@ -341,9 +371,6 @@ class RunRecorder:
         if self._camera_writer:
             self._camera_writer.release()
             self._upload_file(self.run_dir / "camera.avi", "camera.avi")
-        if self._gazebo_writer:
-            self._gazebo_writer.release()
-            self._upload_file(self.run_dir / "gazebo.avi", "gazebo.avi")
 
         self.stop_logging()
         self._log_file.close()
@@ -369,7 +396,7 @@ class RunRecorder:
             f"[REC]   Duration: {mins}m {secs}s\n"
             f"[REC]   Tracker: {self._tracker_frames} frames\n"
             f"[REC]   Camera:  {self._camera_frames} frames\n"
-            f"[REC]   Gazebo:  {self._gazebo_frames} frames\n"
+            f"[REC]   Gazebo:  via ffmpeg (.mp4)\n"
             f"[REC] ═══════════════════════════════════════\n"
         )
 

src/utils/transforms.py

@@ -1,84 +0,0 @@
-#!/usr/bin/env python3
-"""Coordinate transforms for local navigation."""
-
-import numpy as np
-from scipy.spatial.transform import Rotation
-
-
-def quaternion_to_euler(quat):
-    r = Rotation.from_quat([quat[0], quat[1], quat[2], quat[3]])
-    return r.as_euler('xyz')
-
-
-def euler_to_quaternion(roll, pitch, yaw):
-    r = Rotation.from_euler('xyz', [roll, pitch, yaw])
-    return r.as_quat()
-
-
-def rotation_matrix_from_quaternion(quat):
-    r = Rotation.from_quat([quat[0], quat[1], quat[2], quat[3]])
-    return r.as_matrix()
-
-
-def transform_point(point, translation, rotation_quat):
-    R = rotation_matrix_from_quaternion(rotation_quat)
-    return R @ point + translation
-
-
-def inverse_transform_point(point, translation, rotation_quat):
-    R = rotation_matrix_from_quaternion(rotation_quat)
-    return R.T @ (point - translation)
-
-
-def body_to_world(body_vel, yaw):
-    cos_yaw = np.cos(yaw)
-    sin_yaw = np.sin(yaw)
-    
-    world_vel = np.zeros(3)
-    world_vel[0] = body_vel[0] * cos_yaw - body_vel[1] * sin_yaw
-    world_vel[1] = body_vel[0] * sin_yaw + body_vel[1] * cos_yaw
-    world_vel[2] = body_vel[2]
-    
-    return world_vel
-
-
-def world_to_body(world_vel, yaw):
-    cos_yaw = np.cos(yaw)
-    sin_yaw = np.sin(yaw)
-    
-    body_vel = np.zeros(3)
-    body_vel[0] = world_vel[0] * cos_yaw + world_vel[1] * sin_yaw
-    body_vel[1] = -world_vel[0] * sin_yaw + world_vel[1] * cos_yaw
-    body_vel[2] = world_vel[2]
-    
-    return body_vel
-
-
-def normalize_angle(angle):
-    while angle > np.pi:
-        angle -= 2 * np.pi
-    while angle < -np.pi:
-        angle += 2 * np.pi
-    return angle
-
-
-def angle_difference(angle1, angle2):
-    diff = angle1 - angle2
-    return normalize_angle(diff)
-
-
-def create_transformation_matrix(translation, rotation_quat):
-    T = np.eye(4)
-    T[:3, :3] = rotation_matrix_from_quaternion(rotation_quat)
-    T[:3, 3] = translation
-    return T
-
-
-def invert_transformation_matrix(T):
-    R = T[:3, :3]
-    t = T[:3, 3]
-    
-    T_inv = np.eye(4)
-    T_inv[:3, :3] = R.T
-    T_inv[:3, 3] = -R.T @ t
-    return T_inv

src/vision/__init__.py

@@ -1 +1 @@
-"""Vision processing module for GPS-denied navigation."""
+

src/vision/camera_processor.py

@@ -12,7 +12,6 @@ PF_RGB_INT8 = 3
 PF_BGR_INT8 = 8
 PF_R_FLOAT32 = 13
 
-
 class CameraProcessor:
     def __init__(self, topics=None, show_gui=True):
         self.node = Node()
@@ -90,7 +89,6 @@ class CameraProcessor:
     def get_frame(self, camera_name):
         return self.frames.get(camera_name)
 
-
 def main():
     cameras = "both"
     show_gui = True
@@ -135,6 +133,5 @@ def main():
     else:
         proc.spin_headless()
 
-
 if __name__ == "__main__":
     main()

src/vision/object_detector.py

@@ -30,7 +30,6 @@ ARUCO_DICT = {
 W_RES = 640
 H_RES = 480
 
-
 class ObjectDetector:
     CAM_DEG_FOV = 110
 
@@ -124,7 +123,6 @@ class ObjectDetector:
     def annotate_frame(self, frame, detections):
         annotated = frame.copy()
 
-        # Draw center crosshair
         h, w = frame.shape[:2]
         cx, cy = w // 2, h // 2
         cv2.line(annotated, (cx - 15, cy), (cx + 15, cy), (0, 255, 0), 2)
@@ -147,9 +145,3 @@ class ObjectDetector:
 
         cv2.addWeighted(overlay, 0.4, annotated, 0.6, 0, annotated)
         return annotated
-
-    def get_found_markers(self):
-        return self.found_markers
-
-    def reset(self):
-        self.found_markers = {}

src/vision/optical_flow.py

@@ -1,85 +0,0 @@
-#!/usr/bin/env python3
-"""
-Optical Flow - Velocity estimation from downward camera.
-Lucas-Kanade sparse optical flow for GPS-denied velocity estimation.
-"""
-
-import time
-import cv2
-import numpy as np
-
-
-class OpticalFlowEstimator:
-    def __init__(self, focal_length=500.0, min_altitude=0.3, max_altitude=10.0):
-        self.focal_length = focal_length
-        self.min_altitude = min_altitude
-        self.max_altitude = max_altitude
-
-        self.prev_frame = None
-        self.prev_points = None
-        self.prev_time = None
-        self.current_altitude = 1.0
-
-        self.velocity = np.zeros(2)
-
-        self.lk_params = dict(
-            winSize=(15, 15),
-            maxLevel=3,
-            criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03),
-        )
-
-        self.feature_params = dict(
-            maxCorners=100, qualityLevel=0.3, minDistance=7, blockSize=7
-        )
-
-        self.on_velocity = None
-
-        print("[OF] Optical Flow Estimator initialized")
-
-    def set_altitude(self, altitude):
-        if self.min_altitude < altitude < self.max_altitude:
-            self.current_altitude = altitude
-
-    def process_frame(self, camera_name, frame):
-        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
-        current_time = time.time()
-
-        if self.prev_frame is not None and self.prev_points is not None:
-            new_points, status, error = cv2.calcOpticalFlowPyrLK(
-                self.prev_frame, gray, self.prev_points, None, **self.lk_params
-            )
-
-            if new_points is not None:
-                good_new = new_points[status.flatten() == 1]
-                good_old = self.prev_points[status.flatten() == 1]
-
-                if len(good_new) > 10:
-                    flow = good_new - good_old
-                    avg_flow = np.mean(flow, axis=0)
-
-                    if self.prev_time is not None:
-                        dt = current_time - self.prev_time
-                        if dt > 0:
-                            pixel_velocity = avg_flow / dt
-                            self.velocity[0] = (
-                                pixel_velocity[0]
-                                * self.current_altitude
-                                / self.focal_length
-                            )
-                            self.velocity[1] = (
-                                pixel_velocity[1]
-                                * self.current_altitude
-                                / self.focal_length
-                            )
-
-                            if self.on_velocity:
-                                self.on_velocity(self.velocity)
-
-        self.prev_points = cv2.goodFeaturesToTrack(gray, **self.feature_params)
-        self.prev_frame = gray
-        self.prev_time = current_time
-
-        return self.velocity
-
-    def get_velocity(self):
-        return self.velocity.copy()

src/vision/visual_odometry.py

@@ -1,119 +0,0 @@
-#!/usr/bin/env python3
-"""
-Visual Odometry - Camera-based position estimation without GPS.
-ORB/SIFT feature matching with essential matrix decomposition.
-"""
-
-import time
-import cv2
-import numpy as np
-from scipy.spatial.transform import Rotation
-
-
-class VisualOdometry:
-    def __init__(self, camera_matrix=None, detector_type="ORB", min_features=100):
-        self.detector_type = detector_type
-        self.min_features = min_features
-
-        if camera_matrix is not None:
-            self.camera_matrix = np.array(camera_matrix).reshape(3, 3)
-        else:
-            self.camera_matrix = np.array(
-                [[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]]
-            )
-        self.dist_coeffs = np.zeros(5)
-
-        if detector_type == "ORB":
-            self.detector = cv2.ORB_create(nfeatures=500)
-            self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
-        elif detector_type == "SIFT":
-            self.detector = cv2.SIFT_create(nfeatures=500)
-            self.matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=False)
-        else:
-            self.detector = cv2.ORB_create(nfeatures=500)
-            self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
-
-        self.prev_frame = None
-        self.prev_keypoints = None
-        self.prev_descriptors = None
-        self.prev_time = None
-
-        self.position = np.zeros(3)
-        self.orientation = np.eye(3)
-        self.velocity = np.zeros(3)
-
-        self.on_pose = None
-
-        print(f"[VO] Visual Odometry initialized ({detector_type})")
-
-    def process_frame(self, camera_name, frame):
-        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
-        keypoints, descriptors = self.detector.detectAndCompute(gray, None)
-        current_time = time.time()
-
-        if (
-            self.prev_frame is not None
-            and self.prev_descriptors is not None
-            and len(keypoints) >= self.min_features
-        ):
-            matches = self._match_features(self.prev_descriptors, descriptors)
-            if len(matches) >= self.min_features:
-                self._estimate_motion(
-                    self.prev_keypoints, keypoints, matches, current_time
-                )
-
-        self.prev_frame = gray
-        self.prev_keypoints = keypoints
-        self.prev_descriptors = descriptors
-        self.prev_time = current_time
-
-    def _match_features(self, desc1, desc2):
-        if desc1 is None or desc2 is None:
-            return []
-
-        matches = self.matcher.knnMatch(desc1, desc2, k=2)
-        good = []
-        for pair in matches:
-            if len(pair) == 2:
-                m, n = pair
-                if m.distance < 0.7 * n.distance:
-                    good.append(m)
-        return good
-
-    def _estimate_motion(self, prev_kp, curr_kp, matches, current_time):
-        if len(matches) < 5:
-            return
-
-        pts1 = np.float32([prev_kp[m.queryIdx].pt for m in matches])
-        pts2 = np.float32([curr_kp[m.trainIdx].pt for m in matches])
-
-        E, mask = cv2.findEssentialMat(
-            pts1, pts2, self.camera_matrix, method=cv2.RANSAC, prob=0.999, threshold=1.0
-        )
-
-        if E is None:
-            return
-
-        _, R, t, mask = cv2.recoverPose(E, pts1, pts2, self.camera_matrix, mask=mask)
-
-        scale = 1.0
-        translation = scale * t.flatten()
-
-        if self.prev_time is not None:
-            dt = current_time - self.prev_time
-            if dt > 0:
-                self.velocity = translation / dt
-
-        self.position += self.orientation @ translation
-        self.orientation = R @ self.orientation
-
-        if self.on_pose:
-            r = Rotation.from_matrix(self.orientation)
-            self.on_pose(self.position.copy(), r.as_quat())
-
-    def get_pose(self):
-        r = Rotation.from_matrix(self.orientation)
-        return self.position.copy(), r.as_quat()
-
-    def get_velocity(self):
-        return self.velocity.copy()

src/vision/visual_servoing.py

@@ -1,87 +0,0 @@
-#!/usr/bin/env python3
-
-import numpy as np
-
-
-class VisualServoing:
-    def __init__(self, controller, target_marker_id=0, land_altitude=0.5):
-        self.controller = controller
-        self.target_marker_id = target_marker_id
-        self.land_altitude = land_altitude
-
-        self.kp_xy = 0.5
-        self.kp_z = 0.3
-        self.max_velocity = 0.5
-        self.center_tolerance = 30
-        self.land_distance = 1.0
-
-        self.enabled = False
-        self.target_acquired = False
-        self.centered = False
-        self.image_center = (320, 240)
-        self.last_detection = None
-
-        print(f"[VS] Visual Servoing initialized (target marker: {target_marker_id})")
-
-    def enable(self):
-        self.enabled = True
-        print("[VS] Enabled")
-
-    def disable(self):
-        self.enabled = False
-        print("[VS] Disabled")
-
-    def process_detections(self, detections):
-        if not self.enabled:
-            return False
-
-        target = None
-        for d in detections:
-            if d.get("id") == self.target_marker_id:
-                target = d
-                break
-
-        if target is None:
-            self.target_acquired = False
-            return False
-
-        self.target_acquired = True
-        self.last_detection = target
-        return self.compute_control(target)
-
-    def compute_control(self, detection):
-        center_px = detection["center_px"]
-        distance = detection["distance"]
-
-        error_x = self.image_center[0] - center_px[0]
-        error_y = self.image_center[1] - center_px[1]
-
-        self.centered = (abs(error_x) < self.center_tolerance and
-                         abs(error_y) < self.center_tolerance)
-
-        if self.centered and distance < self.land_distance:
-            print(f"\n[VS] Centered and close enough ({distance:.2f}m) - landing")
-            self.controller.land()
-            return True
-
-        vx = np.clip(self.kp_xy * error_y / 100.0, -self.max_velocity, self.max_velocity)
-        vy = np.clip(self.kp_xy * error_x / 100.0, -self.max_velocity, self.max_velocity)
-
-        descend_rate = 0.0
-        if self.centered:
-            descend_rate = min(self.kp_z * distance, self.max_velocity)
-
-        self.controller.update_state()
-        target_z = -(self.controller.altitude - descend_rate)
-        self.controller.move_vel_rel_alt(vx, vy, target_z)
-
-        print(
-            f"\r[VS] ID:{detection['id']} "
-            f"d:{distance:.2f}m "
-            f"err:({error_x:.0f},{error_y:.0f}) "
-            f"vel:({vx:.2f},{vy:.2f}) "
-            f"{'CENTERED' if self.centered else 'ALIGNING'}",
-            end="",
-            flush=True,
-        )
-        return False