110 lines
3.0 KiB
Bash
Executable File
110 lines
3.0 KiB
Bash
Executable File
#!/bin/bash
|
|
# =============================================================================
|
|
# ArduPilot Controller Launcher
|
|
# =============================================================================
|
|
# Starts ArduPilot SITL and your drone controller together.
|
|
#
|
|
# Usage:
|
|
# Terminal 1: ./scripts/run_ardupilot_sim.sh runway
|
|
# Terminal 2: ./scripts/run_ardupilot_controller.sh
|
|
# =============================================================================
|
|
|
|
set -e
|
|
|
|
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
|
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
|
|
|
|
# Deactivate any existing virtual environment to avoid conflicts
|
|
if [ -n "$VIRTUAL_ENV" ]; then
|
|
deactivate 2>/dev/null || true
|
|
fi
|
|
|
|
# Source ArduPilot environment
|
|
if [ -f "$HOME/.ardupilot_env" ]; then
|
|
source "$HOME/.ardupilot_env"
|
|
fi
|
|
|
|
# Activate ArduPilot venv (has empy and other dependencies)
|
|
if [ -f "$HOME/venv-ardupilot/bin/activate" ]; then
|
|
source "$HOME/venv-ardupilot/bin/activate"
|
|
fi
|
|
|
|
echo "=============================================="
|
|
echo " ArduPilot Controller"
|
|
echo "=============================================="
|
|
echo ""
|
|
|
|
# Check dependencies
|
|
if ! command -v sim_vehicle.py &> /dev/null; then
|
|
echo "[ERROR] sim_vehicle.py not found"
|
|
echo ""
|
|
echo "Fix: Add ArduPilot tools to PATH:"
|
|
echo " export PATH=\$PATH:~/ardupilot/Tools/autotest"
|
|
echo ""
|
|
echo "Or source the ArduPilot environment:"
|
|
echo " source ~/.ardupilot_env"
|
|
exit 1
|
|
fi
|
|
|
|
# Verify empy is available
|
|
if ! python3 -c "import em" 2>/dev/null; then
|
|
echo "[ERROR] empy not found"
|
|
echo ""
|
|
echo "Fix: Install empy in ArduPilot venv:"
|
|
echo " source ~/venv-ardupilot/bin/activate"
|
|
echo " pip install empy==3.3.4"
|
|
exit 1
|
|
fi
|
|
|
|
echo "[INFO] Environment OK"
|
|
echo "[INFO] Python: $(which python3)"
|
|
echo ""
|
|
|
|
# Start SITL with console output
|
|
echo "[INFO] Starting ArduPilot SITL..."
|
|
echo "[INFO] This will build ArduCopter if needed (first run takes ~2 min)"
|
|
echo ""
|
|
|
|
# Run sim_vehicle in a way that shows output
|
|
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map &
|
|
SITL_PID=$!
|
|
|
|
# Wait for SITL to be ready (check for TCP port 5760)
|
|
echo "[INFO] Waiting for SITL to start (TCP 5760)..."
|
|
TRIES=0
|
|
MAX_TRIES=60 # 60 seconds max
|
|
while ! nc -z 127.0.0.1 5760 2>/dev/null; do
|
|
sleep 1
|
|
TRIES=$((TRIES + 1))
|
|
if [ $TRIES -ge $MAX_TRIES ]; then
|
|
echo "[ERROR] Timeout waiting for SITL on port 5760"
|
|
kill $SITL_PID 2>/dev/null || true
|
|
exit 1
|
|
fi
|
|
# Check if SITL process is still running
|
|
if ! kill -0 $SITL_PID 2>/dev/null; then
|
|
echo "[ERROR] SITL process died"
|
|
exit 1
|
|
fi
|
|
echo -n "."
|
|
done
|
|
echo ""
|
|
echo "[OK] SITL is ready on TCP 5760"
|
|
echo ""
|
|
|
|
# Give it a moment more to stabilize
|
|
sleep 2
|
|
|
|
# Trap to kill SITL on exit
|
|
trap "echo ''; echo '[INFO] Stopping SITL...'; kill $SITL_PID 2>/dev/null; exit 0" INT TERM
|
|
|
|
# Run controller
|
|
echo "[INFO] Starting drone controller..."
|
|
echo "[INFO] Press Ctrl+C to stop"
|
|
echo ""
|
|
cd "$PROJECT_DIR"
|
|
python scripts/run_ardupilot.py "$@"
|
|
|
|
# Cleanup
|
|
kill $SITL_PID 2>/dev/null
|