diff --git a/README.md b/README.md index bca623e..3ebc856 100644 --- a/README.md +++ b/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 diff --git a/config/search.yaml b/config/search.yaml index 6dfedf0..c764dfd 100644 --- a/config/search.yaml +++ b/config/search.yaml @@ -1,37 +1,23 @@ -# ─── Mission / Search Configuration ────────────────────────── -# Single source of truth for all mission parameters. - -# ── Flight ─────────────────────────────────────────────────── -altitude: 5.0 # Search altitude (feet) - -# ── ArUco Marker ───────────────────────────────────────────── +altitude: 5.0 marker: dictionary: DICT_4X4_50 - size: 0.5 # Physical marker size in meters - landing_ids: [0] # Marker IDs that trigger landing (on UGV) - target_ids: [1] # Marker IDs to find and report to UGV - target_position: [5.0, 3.0] # Initial X, Y location of the target Aruco map in the map - -# ── Search Patterns ────────────────────────────────────────── + size: 0.5 + landing_ids: [0] + target_ids: [1] + target_position: [5.0, 3.0] 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] diff --git a/config/uav.yaml b/config/uav.yaml index 4d90383..d5dd4db 100644 --- a/config/uav.yaml +++ b/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 diff --git a/config/ugv.yaml b/config/ugv.yaml index 3e774df..2f58176 100644 --- a/config/ugv.yaml +++ b/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 diff --git a/docs/setup_guide.md b/docs/setup_guide.md index c484908..dff9d8a 100644 --- a/docs/setup_guide.md +++ b/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 diff --git a/docs/wsl_setup_guide.md b/docs/wsl_setup_guide.md index 158f395..79aa0c3 100644 --- a/docs/wsl_setup_guide.md +++ b/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 diff --git a/scripts/generate_tags.py b/scripts/generate_tags.py index daf4861..04540a2 100644 --- a/scripts/generate_tags.py +++ b/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())) @@ -113,7 +110,7 @@ def main(): params = cv2.aruco.DetectorParameters() except AttributeError: params = cv2.aruco.DetectorParameters_create() - + params.minMarkerPerimeterRate = 0.01 try: params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX @@ -132,9 +129,8 @@ def main(): _, detected_ids, _ = _detect(img) if detected_ids is not None and marker_id in detected_ids.flatten(): ok += 1 - - print(f"Done. {ok}/{len(ids)} markers verified.") + print(f"Done. {ok}/{len(ids)} markers verified.") if __name__ == "__main__": main() diff --git a/scripts/generate_world.py b/scripts/generate_world.py index 362007d..357a80d 100755 --- a/scripts/generate_world.py +++ b/scripts/generate_world.py @@ -22,10 +22,10 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav ugv_x = ugv_cfg.get("position", {}).get("x", 0.0) ugv_y = ugv_cfg.get("position", {}).get("y", 0.0) - + 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]) @@ -42,19 +42,19 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav try: tree = ET.parse(base_path) root = tree.getroot() - + world = root.find('world') if world is None: print("[ERROR] No tag found in SDF.") sys.exit(1) - + for include in world.findall('include'): uri = include.find('uri') if uri is not None and uri.text == 'model://iris_with_gimbal': pose = include.find('pose') if pose is not None: pose.text = f"{uav_x} {uav_y} {uav_z} 0 0 90" - + name = include.find('name') if name is not None and name.text == 'ugv': pose = include.find('pose') @@ -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) @@ -80,29 +80,29 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav gf_model = ET.SubElement(world, "model", name="geofence_visual") ET.SubElement(gf_model, "static").text = "true" link = ET.SubElement(gf_model, "link", name="link") - + import math for i in range(len(points)): p1 = points[i] p2 = points[(i + 1) % len(points)] x1, y1 = float(p1[0]), float(p1[1]) x2, y2 = float(p2[0]), float(p2[1]) - + dx = x2 - x1 dy = y2 - y1 length = math.sqrt(dx*dx + dy*dy) cx = x1 + dx / 2.0 cy = y1 + dy / 2.0 yaw = math.atan2(dy, dx) - + visual = ET.SubElement(link, "visual", name=f"edge_{i}") ET.SubElement(visual, "pose").text = f"{cx} {cy} 0.01 0 0 {yaw}" - + 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") ET.SubElement(material, "ambient").text = "1 0 0 1" ET.SubElement(material, "diffuse").text = "1 0 0 1" @@ -112,7 +112,7 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav print(f"[INFO] Generated world file: {output_path}") print(f"[INFO] UGV set to ({ugv_x}, {ugv_y})") print(f"[INFO] Target Marker set to ({target_x}, {target_y})") - + except Exception as e: print(f"[ERROR] Failed to parse or write XML: {e}") sys.exit(1) diff --git a/scripts/kill_simulation.sh b/scripts/kill_simulation.sh index d8c5dba..ab7bff1 100755 --- a/scripts/kill_simulation.sh +++ b/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" diff --git a/scripts/run_autonomous.sh b/scripts/run_autonomous.sh index a8dcb0a..09e6561 100755 --- a/scripts/run_autonomous.sh +++ b/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|true|false|' "$GZ_USER_GUI" sed -i 's|show_again="true"|show_again="false"|' "$GZ_USER_GUI" - cat << 'EOF' >> "$GZ_USER_GUI" @@ -141,7 +120,6 @@ cat << 'EOF' >> "$GZ_USER_GUI" false #777777 - true true @@ -149,58 +127,45 @@ cat << 'EOF' >> "$GZ_USER_GUI" 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 diff --git a/scripts/run_simulation.sh b/scripts/run_simulation.sh index 117a45d..8715852 100755 --- a/scripts/run_simulation.sh +++ b/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 " arm throttle # Arm the drone" -echo " takeoff 5 # Takeoff to 5 meters" -echo " guided 10 5 -10 # Fly to position (N, E, D)" -echo " rtl # Return to launch" -echo " land # Land" +echo " mode guided +echo " arm throttle +echo " takeoff 5 +echo " guided 10 5 -10 +echo " rtl +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 \ diff --git a/scripts/setup_gazebo_nvidia.sh b/scripts/setup_gazebo_nvidia.sh index ef4bcd8..249e042 100755 --- a/scripts/setup_gazebo_nvidia.sh +++ b/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" diff --git a/scripts/uninstall.sh b/scripts/uninstall.sh index fa35139..159ca99 100755 --- a/scripts/uninstall.sh +++ b/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}" diff --git a/setup.sh b/setup.sh index 1cb917a..f66781f 100755 --- a/setup.sh +++ b/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 - -# WSL-specific packages + netcat-openbsd \ + ffmpeg \ + 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" diff --git a/src/__init__.py b/src/__init__.py index e69de29..8b13789 100644 --- a/src/__init__.py +++ b/src/__init__.py @@ -0,0 +1 @@ + diff --git a/src/control/__init__.py b/src/control/__init__.py index e69de29..8b13789 100644 --- a/src/control/__init__.py +++ b/src/control/__init__.py @@ -0,0 +1 @@ + diff --git a/src/control/mission_planner.py b/src/control/mission_planner.py deleted file mode 100644 index 49b7834..0000000 --- a/src/control/mission_planner.py +++ /dev/null @@ -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() diff --git a/src/control/uav_controller.py b/src/control/uav_controller.py index 9a31fea..a12d470 100644 --- a/src/control/uav_controller.py +++ b/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_GPS_CHECK': 0, # Disable GPS preflight checks in SITL - 'EK3_POSNE_M_NSE': 0.5, # Trust GPS position more - 'EK3_VELD_M_NSE': 0.2, # Trust GPS vertical vel more (default 0.4) - 'EK3_VELNE_M_NSE': 0.1, # Trust GPS horizontal vel more (default 0.3) - 'INS_GYRO_CAL': 0, # Skip gyro cal on boot (SITL doesn't need it) - 'ARMING_CHECK': 0, # Disable ALL preflight checks (SITL only) + 'EK3_CHECK_SCALE': 200, + 'EK3_GPS_CHECK': 0, + 'EK3_POSNE_M_NSE': 0.5, + 'EK3_VELD_M_NSE': 0.2, + 'EK3_VELNE_M_NSE': 0.1, + 'INS_GYRO_CAL': 0, + '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): - self.conn.mav.command_long_send( - self.conn.target_system, - self.conn.target_component, - mavlink.MAV_CMD_DO_CHANGE_SPEED, - 0, 1, speed, -1, 0, 0, 0, 0) +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)") diff --git a/src/control/ugv_controller.py b/src/control/ugv_controller.py index 5281c22..c6b1baf 100644 --- a/src/control/ugv_controller.py +++ b/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 diff --git a/src/main.py b/src/main.py index dc27e01..5940304 100644 --- a/src/main.py +++ b/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.search import Search +from control.uav_controller import Controller, setup_ardupilot, wait_for_landing +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, @@ -188,12 +142,11 @@ def main(): tracker.start() 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) - + setup_ardupilot(ctrl) if recorder: recorder.set_phase("ekf_wait") @@ -211,17 +164,15 @@ def main(): ctrl.takeoff(altitude) tracker.reset_timer() - + if recorder: recorder.set_phase("search") ctrl.wait_altitude(altitude, tolerance=1.0, timeout=30) - + 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 --- - # Shared state: set by callback, read by post-search logic - ugv_target_pos = [None, None] # [x, y] — set when target found + ugv_target_pos = [None, None] 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() @@ -329,11 +269,10 @@ def main(): print(f"[MAIN] Above UGV — initiating landing") if recorder: recorder.set_phase("landing") - + 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() diff --git a/src/navigation/__init__.py b/src/navigation/__init__.py index 20b8ecd..8b13789 100644 --- a/src/navigation/__init__.py +++ b/src/navigation/__init__.py @@ -1 +1 @@ -"""Navigation module for local path planning and waypoint following.""" + diff --git a/src/navigation/flight_tracker.py b/src/navigation/flight_tracker.py index b4fede9..f81451a 100644 --- a/src/navigation/flight_tracker.py +++ b/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: diff --git a/src/navigation/local_planner.py b/src/navigation/local_planner.py deleted file mode 100644 index 3db426f..0000000 --- a/src/navigation/local_planner.py +++ /dev/null @@ -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() diff --git a/src/navigation/obstacle_avoidance.py b/src/navigation/obstacle_avoidance.py deleted file mode 100644 index b301db0..0000000 --- a/src/navigation/obstacle_avoidance.py +++ /dev/null @@ -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() diff --git a/src/navigation/patterns/lawnmower.py b/src/navigation/patterns/lawnmower.py new file mode 100644 index 0000000..aa0f9d7 --- /dev/null +++ b/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 diff --git a/src/navigation/patterns/levy.py b/src/navigation/patterns/levy.py new file mode 100644 index 0000000..75cac0e --- /dev/null +++ b/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 diff --git a/src/navigation/patterns/spiral.py b/src/navigation/patterns/spiral.py new file mode 100644 index 0000000..b84966e --- /dev/null +++ b/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 diff --git a/src/control/search.py b/src/navigation/search.py similarity index 70% rename from src/control/search.py rename to src/navigation/search.py index 834e6b9..c5475b5 100644 --- a/src/control/search.py +++ b/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 - self.spiral_leg_increment = self.cam_fov_meters * 0.8 + overlap_spacing = 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() @@ -305,9 +200,9 @@ class Search: if marker_id in self.target_ids and marker_id not in self._dispatched_targets: self._dispatched_targets.add(marker_id) self.running = False - + final_pos = self.center_over_target(marker_id) - + print(f"\n[SEARCH] Target ID:{marker_id} centered! " f"Dispatching UGV to ({final_pos['x']:.1f}, {final_pos['y']:.1f})") if self.on_target_found: @@ -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 @@ -418,7 +313,7 @@ class Search: hfov, center_px, max_corrections, max_lost, gain, allowed_ids=None): if allowed_ids is None: allowed_ids = self.land_ids - + lost_count = 0 centered = False @@ -466,15 +361,13 @@ class Search: alt = max(self.ctrl.altitude, 0.5) 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) @@ -494,9 +387,9 @@ class Search: self.ctrl.update_state() pos = self.ctrl.get_local_position() 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): diff --git a/src/navigation/waypoint_follower.py b/src/navigation/waypoint_follower.py deleted file mode 100644 index 1be97df..0000000 --- a/src/navigation/waypoint_follower.py +++ /dev/null @@ -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() diff --git a/src/safety/__init__.py b/src/safety/__init__.py index be85e0f..8b13789 100644 --- a/src/safety/__init__.py +++ b/src/safety/__init__.py @@ -1 +1 @@ -"""Safety module for geofencing and failsafe handling.""" + diff --git a/src/safety/failsafe_handler.py b/src/safety/failsafe_handler.py deleted file mode 100644 index 04d3ac6..0000000 --- a/src/safety/failsafe_handler.py +++ /dev/null @@ -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() diff --git a/src/safety/geofence.py b/src/safety/geofence.py new file mode 100644 index 0000000..1a75f99 --- /dev/null +++ b/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 diff --git a/src/safety/geofence_monitor.py b/src/safety/geofence_monitor.py deleted file mode 100644 index ed8041a..0000000 --- a/src/safety/geofence_monitor.py +++ /dev/null @@ -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() diff --git a/src/utils/__init__.py b/src/utils/__init__.py index e69de29..8b13789 100644 --- a/src/utils/__init__.py +++ b/src/utils/__init__.py @@ -0,0 +1 @@ + diff --git a/src/utils/config.py b/src/utils/config.py new file mode 100644 index 0000000..3c1ddfc --- /dev/null +++ b/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 {} + diff --git a/src/utils/convert.py b/src/utils/convert.py index 1c669cc..b385648 100644 --- a/src/utils/convert.py +++ b/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 diff --git a/src/utils/helpers.py b/src/utils/helpers.py index 1a2e935..8bcbe42 100644 --- a/src/utils/helpers.py +++ b/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 diff --git a/src/utils/recorder.py b/src/utils/recorder.py index 8e27eb0..f7b2fa4 100644 --- a/src/utils/recorder.py +++ b/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 @@ -49,10 +45,10 @@ class RunRecorder: config_data[key] = data except Exception as e: print(f"[REC] Failed to load config {cfg}: {e}") - + if "ugv" in config_data and "topics" in config_data["ugv"]: del config_data["ugv"]["topics"] - + if "uav" in config_data and "connection" in config_data["uav"]: del config_data["uav"]["connection"] @@ -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}") @@ -72,9 +68,9 @@ class RunRecorder: else: self.run_dir = project_dir / "results" / self.sim_name self.run_dir.mkdir(parents=True, exist_ok=True) - + self.run_num = self.sim_id - + print(f"[REC] Recording local results to {self.run_dir.name} and API ({self.sim_name}, ID: {self.sim_id})") self._log_path = self.run_dir / "log.txt" self._log_file = open(self._log_path, "w") @@ -86,7 +82,7 @@ class RunRecorder: self._tracker_writer = None self._camera_writer = None self._gazebo_writer = None - + self._tracker_size = None self._camera_size = None self._gazebo_size = None @@ -105,21 +101,18 @@ 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): if not self.sim_id: return - + payload = { "cpu_info": "Unknown CPU", "gpu_info": "Unknown GPU", "ram_info": "Unknown RAM" } - + try: import subprocess cpu = "Unknown" @@ -127,19 +120,19 @@ class RunRecorder: cpu = subprocess.check_output("grep -m 1 'model name' /proc/cpuinfo | cut -d ':' -f 2", shell=True, timeout=2).decode('utf-8').strip() except Exception: pass if cpu: payload["cpu_info"] = cpu - + try: ram_kb = int(subprocess.check_output("awk '/MemTotal/ {print $2}' /proc/meminfo", shell=True, timeout=2)) payload["ram_info"] = f"{round(ram_kb / 1024 / 1024, 1)} GB" except Exception: pass - + try: gpu = subprocess.check_output("lspci | grep -i vga | cut -d ':' -f 3", shell=True, timeout=2).decode('utf-8').strip() if gpu: payload["gpu_info"] = gpu except Exception: pass except Exception: pass - + try: requests.put(f"{API_URL}/api/simulations/{self.sim_id}/hardware", json=payload, timeout=5) except Exception as e: @@ -151,8 +144,8 @@ class RunRecorder: try: with open(path, 'rb') as f: resp = requests.post( - f"{API_URL}/api/simulations/{self.sim_id}/upload", - files={"file": (filename, f)}, + f"{API_URL}/api/simulations/{self.sim_id}/upload", + files={"file": (filename, f)}, timeout=60 ) if resp.status_code == 200: @@ -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 + return None wid_line = lines[0] wid_match = re.search(r'(0x[0-9a-fA-F]+)', wid_line) - if not wid_match: - return - self._gazebo_wid = wid_match.group(1) - print(f"[REC] Found Gazebo window ID: {self._gazebo_wid}") - except Exception as e: - print(f"[REC] Error finding Gazebo window: {e}") + if wid_match: + return wid_match.group(1) + except Exception: + pass + return None + + 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": @@ -201,9 +212,57 @@ class RunRecorder: self._tracker_ref = tracker self._camera_ref = camera self._recording = True - - if not self._gazebo_wid: - self._find_gazebo_window() + + window_id = self._find_gazebo_window_ffmpeg() + 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() @@ -356,8 +383,8 @@ class RunRecorder: if self.sim_id: try: requests.put( - f"{API_URL}/api/simulations/{self.sim_id}/time", - json={"search_time": self.search_duration, "total_time": duration}, + f"{API_URL}/api/simulations/{self.sim_id}/time", + json={"search_time": self.search_duration, "total_time": duration}, timeout=5 ) except Exception as e: @@ -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" ) diff --git a/src/utils/transforms.py b/src/utils/transforms.py deleted file mode 100644 index 96f7081..0000000 --- a/src/utils/transforms.py +++ /dev/null @@ -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 diff --git a/src/vision/__init__.py b/src/vision/__init__.py index 103157c..8b13789 100644 --- a/src/vision/__init__.py +++ b/src/vision/__init__.py @@ -1 +1 @@ -"""Vision processing module for GPS-denied navigation.""" + diff --git a/src/vision/camera_processor.py b/src/vision/camera_processor.py index 83be650..03dee16 100644 --- a/src/vision/camera_processor.py +++ b/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() diff --git a/src/vision/object_detector.py b/src/vision/object_detector.py index 8d8ad96..7e8572a 100644 --- a/src/vision/object_detector.py +++ b/src/vision/object_detector.py @@ -30,7 +30,6 @@ ARUCO_DICT = { W_RES = 640 H_RES = 480 - class ObjectDetector: CAM_DEG_FOV = 110 @@ -123,8 +122,7 @@ 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 = {} diff --git a/src/vision/optical_flow.py b/src/vision/optical_flow.py deleted file mode 100644 index 54a66d3..0000000 --- a/src/vision/optical_flow.py +++ /dev/null @@ -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() diff --git a/src/vision/visual_odometry.py b/src/vision/visual_odometry.py deleted file mode 100644 index db9ce95..0000000 --- a/src/vision/visual_odometry.py +++ /dev/null @@ -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() diff --git a/src/vision/visual_servoing.py b/src/vision/visual_servoing.py deleted file mode 100644 index 51a381b..0000000 --- a/src/vision/visual_servoing.py +++ /dev/null @@ -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