204 lines
7.3 KiB
Bash
Executable File
204 lines
7.3 KiB
Bash
Executable File
#!/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 ;;
|
|
--no-software-render) SOFTWARE_RENDER=false; shift ;;
|
|
--world) WORLD="$2"; shift 2 ;;
|
|
--search) SEARCH="$2"; shift 2 ;;
|
|
--altitude) ALTITUDE="$2"; shift 2 ;;
|
|
-h|--help)
|
|
echo "Usage: $0 [options]"
|
|
echo " --software-render Force software rendering"
|
|
echo " --no-software-render Disable software rendering"
|
|
echo " --world <file> World file (default: uav_ugv_search.sdf)"
|
|
echo " --search <type> spiral, lawnmower, levy (default: spiral)"
|
|
echo " --altitude <m> Override altitude from config"
|
|
exit 0
|
|
;;
|
|
*) shift ;;
|
|
esac
|
|
done
|
|
cleanup_all() {
|
|
print_info "Cleaning up ..."
|
|
pkill -f "camera_viewer.py" 2>/dev/null || true
|
|
pkill -f "camera_processor" 2>/dev/null || true
|
|
pkill -f "gz sim" 2>/dev/null || true
|
|
pkill -f "arducopter" 2>/dev/null || true
|
|
pkill -f "sim_vehicle.py" 2>/dev/null || true
|
|
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
|
|
pkill -f "mavproxy" 2>/dev/null || true
|
|
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"
|
|
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"
|
|
SOFTWARE_RENDER=true
|
|
else
|
|
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"
|
|
elif [ -f "$HOME/ardupilot_gazebo/worlds/$WORLD" ]; then
|
|
WORLD_FILE="$HOME/ardupilot_gazebo/worlds/$WORLD"
|
|
elif [ -f "$WORLD" ]; then
|
|
WORLD_FILE="$WORLD"
|
|
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 "Search: $SEARCH"
|
|
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>
|
|
<property key="resizable" type="bool">false</property>
|
|
<property key="x" type="double">300</property>
|
|
<property key="y" type="double">50</property>
|
|
<property key="width" type="double">50</property>
|
|
<property key="height" type="double">50</property>
|
|
<property key="state" type="string">floating</property>
|
|
<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>
|
|
<bitrate>4000000</bitrate>
|
|
</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 to start ..."
|
|
SITL_TIMEOUT=60
|
|
SITL_WAITED=0
|
|
while ! pgrep -f "arducopter" > /dev/null 2>&1; do
|
|
sleep 2
|
|
SITL_WAITED=$((SITL_WAITED + 2))
|
|
if [ $SITL_WAITED -ge $SITL_TIMEOUT ]; then
|
|
print_error "ArduPilot SITL failed to start after ${SITL_TIMEOUT}s"
|
|
exit 1
|
|
fi
|
|
print_info " Waiting for arducopter process ... (${SITL_WAITED}s)"
|
|
done
|
|
sleep 5
|
|
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 viewers ..."
|
|
python3 -W ignore:RuntimeWarning -m src.vision.camera_processor down &
|
|
CAM_PID=$!
|
|
python3 -W ignore:RuntimeWarning -m src.vision.camera_processor ugv &
|
|
UGV_CAM_PID=$!
|
|
print_success "Camera viewers running (UAV: $CAM_PID, UGV: $UGV_CAM_PID)"
|
|
print_info ""
|
|
print_info "==================================="
|
|
print_info " Simulation Running"
|
|
print_info "==================================="
|
|
print_info " Gazebo -> ArduPilot SITL (TCP:5760)"
|
|
print_info " |"
|
|
print_info " main.py (pymavlink + gz.transport)"
|
|
print_info " | |"
|
|
print_info " UAV ctrl UGV ctrl (/ugv/cmd_vel)"
|
|
print_info ""
|
|
print_info " UAV starts ON UGV, takes off, searches for"
|
|
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!"
|
|
print_info "=================================="
|
|
print_info "Gazebo GUI still open."
|
|
print_info "Press Ctrl+C to exit."
|
|
print_info "=================================="
|
|
print_info ""
|
|
wait $GZ_PID 2>/dev/null
|