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

scripts/run_autonomous.sh

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