feat: Simplify drone controller to basic flight pattern, overhaul installation guide, and update related scripts.
This commit is contained in:
@@ -1,351 +1,23 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
ArduPilot Drone Controller
|
||||
==========================
|
||||
Connects your drone_controller.py logic to ArduPilot SITL via MAVLink.
|
||||
ArduPilot Flight Demo
|
||||
=====================
|
||||
Simple script to make the drone takeoff, fly a pattern, and land.
|
||||
|
||||
Usage:
|
||||
Terminal 1: ./scripts/run_ardupilot_sim.sh runway
|
||||
Terminal 2: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
|
||||
Terminal 3: python run_ardupilot.py
|
||||
|
||||
This script:
|
||||
1. Connects to ArduPilot SITL via MAVLink
|
||||
2. Gets telemetry (position, attitude, velocity)
|
||||
3. Runs your drone_controller.calculate_landing_maneuver()
|
||||
4. Sends velocity commands to ArduPilot
|
||||
Terminal 3: python scripts/run_ardupilot.py --pattern square
|
||||
"""
|
||||
|
||||
import sys
|
||||
import os
|
||||
|
||||
# Add project root to path
|
||||
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
|
||||
import time
|
||||
import math
|
||||
import argparse
|
||||
|
||||
try:
|
||||
from pymavlink import mavutil
|
||||
except ImportError:
|
||||
print("ERROR: pymavlink not installed")
|
||||
print("Run: pip install pymavlink")
|
||||
sys.exit(1)
|
||||
|
||||
from config import ARDUPILOT, CONTROLLER, MAVLINK
|
||||
|
||||
|
||||
class ArduPilotInterface:
|
||||
"""Interface to ArduPilot SITL via MAVLink."""
|
||||
|
||||
def __init__(self, connection_string=None):
|
||||
self.connection_string = connection_string or MAVLINK["connection_string"]
|
||||
self.mav = None
|
||||
self.armed = False
|
||||
self.mode = "UNKNOWN"
|
||||
|
||||
# Telemetry state
|
||||
self.position = {"x": 0, "y": 0, "z": 0}
|
||||
self.velocity = {"x": 0, "y": 0, "z": 0}
|
||||
self.attitude = {"roll": 0, "pitch": 0, "yaw": 0}
|
||||
self.altitude = 0
|
||||
self.vertical_velocity = 0
|
||||
|
||||
def connect(self, timeout=30):
|
||||
"""Connect to ArduPilot SITL."""
|
||||
print(f"[INFO] Connecting to {self.connection_string}...")
|
||||
|
||||
try:
|
||||
self.mav = mavutil.mavlink_connection(self.connection_string)
|
||||
self.mav.wait_heartbeat(timeout=timeout)
|
||||
print(f"[OK] Connected to system {self.mav.target_system}")
|
||||
return True
|
||||
except Exception as e:
|
||||
print(f"[ERROR] Connection failed: {e}")
|
||||
return False
|
||||
|
||||
def update_telemetry(self):
|
||||
"""Read and update telemetry from ArduPilot."""
|
||||
# Read all available messages
|
||||
while True:
|
||||
msg = self.mav.recv_match(blocking=False)
|
||||
if msg is None:
|
||||
break
|
||||
|
||||
msg_type = msg.get_type()
|
||||
|
||||
if msg_type == "HEARTBEAT":
|
||||
self.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
|
||||
# Get mode name using mavutil helper
|
||||
try:
|
||||
self.mode = mavutil.mode_string_v10(msg)
|
||||
except Exception:
|
||||
self.mode = str(msg.custom_mode)
|
||||
|
||||
elif msg_type == "LOCAL_POSITION_NED":
|
||||
self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
|
||||
self.velocity = {"x": msg.vx, "y": msg.vy, "z": msg.vz}
|
||||
self.altitude = -msg.z # NED: down is positive
|
||||
self.vertical_velocity = -msg.vz
|
||||
|
||||
elif msg_type == "ATTITUDE":
|
||||
self.attitude = {
|
||||
"roll": msg.roll,
|
||||
"pitch": msg.pitch,
|
||||
"yaw": msg.yaw
|
||||
}
|
||||
|
||||
def get_telemetry(self):
|
||||
"""Get telemetry in the same format as standalone simulation."""
|
||||
self.update_telemetry()
|
||||
|
||||
return {
|
||||
"imu": {
|
||||
"orientation": self.attitude,
|
||||
"angular_velocity": {"x": 0, "y": 0, "z": 0} # Not available via MAVLink easily
|
||||
},
|
||||
"altimeter": {
|
||||
"altitude": self.altitude,
|
||||
"vertical_velocity": self.vertical_velocity
|
||||
},
|
||||
"velocity": self.velocity,
|
||||
"landing_pad": None, # Not available in ArduPilot mode - use vision
|
||||
"position": self.position, # Extra: actual position (GPS-like)
|
||||
"armed": self.armed,
|
||||
"mode": self.mode
|
||||
}
|
||||
|
||||
def set_mode(self, mode_name):
|
||||
"""Set flight mode."""
|
||||
mode_map = self.mav.mode_mapping()
|
||||
if mode_name.upper() not in mode_map:
|
||||
print(f"[WARN] Unknown mode: {mode_name}")
|
||||
return False
|
||||
|
||||
mode_id = mode_map[mode_name.upper()]
|
||||
self.mav.set_mode(mode_id)
|
||||
print(f"[OK] Mode set to {mode_name}")
|
||||
return True
|
||||
|
||||
def arm(self, force=True):
|
||||
"""Arm motors."""
|
||||
print("[INFO] Arming...")
|
||||
|
||||
for attempt in range(3):
|
||||
if force:
|
||||
# Force arm (bypass checks)
|
||||
self.mav.mav.command_long_send(
|
||||
self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
0, 1, 21196, 0, 0, 0, 0, 0 # 21196 = force arm
|
||||
)
|
||||
else:
|
||||
self.mav.arducopter_arm()
|
||||
|
||||
# Wait and check if armed
|
||||
time.sleep(1)
|
||||
self.update_telemetry()
|
||||
|
||||
if self.armed:
|
||||
print("[OK] Armed")
|
||||
return True
|
||||
else:
|
||||
print(f"[WARN] Arm attempt {attempt + 1}/3 failed, retrying...")
|
||||
|
||||
print("[ERROR] Failed to arm after 3 attempts")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
"""Disarm motors."""
|
||||
self.mav.arducopter_disarm()
|
||||
print("[OK] Disarmed")
|
||||
|
||||
def takeoff(self, altitude=5.0):
|
||||
"""Takeoff to specified altitude."""
|
||||
self.mav.mav.command_long_send(
|
||||
self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
|
||||
0, 0, 0, 0, 0, 0, 0, altitude
|
||||
)
|
||||
print(f"[OK] Takeoff to {altitude}m")
|
||||
|
||||
def send_velocity(self, vx, vy, vz, yaw_rate=0):
|
||||
"""Send velocity command in body frame.
|
||||
|
||||
Args:
|
||||
vx: Forward velocity (m/s)
|
||||
vy: Right velocity (m/s)
|
||||
vz: Down velocity (m/s, positive = descend)
|
||||
yaw_rate: Yaw rate (rad/s)
|
||||
"""
|
||||
# Type mask: use velocity only
|
||||
type_mask = (
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE
|
||||
)
|
||||
|
||||
self.mav.mav.set_position_target_local_ned_send(
|
||||
0, # time_boot_ms
|
||||
self.mav.target_system,
|
||||
self.mav.target_component,
|
||||
mavutil.mavlink.MAV_FRAME_BODY_NED,
|
||||
type_mask,
|
||||
0, 0, 0, # position (ignored)
|
||||
vx, vy, vz, # velocity
|
||||
0, 0, 0, # acceleration (ignored)
|
||||
0, yaw_rate # yaw, yaw_rate
|
||||
)
|
||||
|
||||
class SimpleController:
|
||||
"""Simple landing controller that doesn't require ROS 2."""
|
||||
|
||||
def __init__(self):
|
||||
self.Kp_z = CONTROLLER.get("Kp_z", 0.5)
|
||||
self.Kd_z = CONTROLLER.get("Kd_z", 0.3)
|
||||
self.Kp_xy = CONTROLLER.get("Kp_xy", 0.3)
|
||||
self.Kd_xy = CONTROLLER.get("Kd_xy", 0.2)
|
||||
|
||||
def calculate_landing_maneuver(self, telemetry, rover_telemetry=None):
|
||||
"""Calculate control outputs for landing."""
|
||||
altimeter = telemetry.get('altimeter', {})
|
||||
altitude = altimeter.get('altitude', 5.0)
|
||||
vertical_vel = altimeter.get('vertical_velocity', 0.0)
|
||||
|
||||
velocity = telemetry.get('velocity', {'x': 0, 'y': 0, 'z': 0})
|
||||
vel_x = velocity.get('x', 0.0)
|
||||
vel_y = velocity.get('y', 0.0)
|
||||
|
||||
landing_pad = telemetry.get('landing_pad', None)
|
||||
|
||||
# Descent rate control
|
||||
if altitude > 1.0:
|
||||
target_descent_rate = -0.5
|
||||
else:
|
||||
target_descent_rate = -0.2
|
||||
|
||||
descent_error = target_descent_rate - vertical_vel
|
||||
thrust = self.Kp_z * descent_error
|
||||
|
||||
# Horizontal control
|
||||
if landing_pad is not None:
|
||||
target_x = landing_pad.get('relative_x', 0.0)
|
||||
target_y = landing_pad.get('relative_y', 0.0)
|
||||
|
||||
pitch = self.Kp_xy * target_x - self.Kd_xy * vel_x
|
||||
roll = self.Kp_xy * target_y - self.Kd_xy * vel_y
|
||||
else:
|
||||
# Just dampen velocity
|
||||
pitch = -self.Kd_xy * vel_x
|
||||
roll = -self.Kd_xy * vel_y
|
||||
|
||||
yaw = 0.0
|
||||
|
||||
return (thrust, pitch, roll, yaw)
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description="ArduPilot Drone Controller")
|
||||
parser.add_argument("--connection", "-c", default=None,
|
||||
help="MAVLink connection string (default: from config.py)")
|
||||
parser.add_argument("--takeoff-alt", "-a", type=float, default=5.0,
|
||||
help="Takeoff altitude (meters)")
|
||||
parser.add_argument("--no-takeoff", action="store_true",
|
||||
help="Don't auto takeoff, just control")
|
||||
args = parser.parse_args()
|
||||
|
||||
# Create ArduPilot interface
|
||||
ardupilot = ArduPilotInterface(args.connection)
|
||||
|
||||
# Connect
|
||||
if not ardupilot.connect():
|
||||
print("[ERROR] Could not connect to ArduPilot")
|
||||
print("Make sure sim_vehicle.py is running")
|
||||
sys.exit(1)
|
||||
|
||||
# Create simple controller (no ROS 2 required)
|
||||
controller = SimpleController()
|
||||
|
||||
print("\n" + "=" * 50)
|
||||
print(" ArduPilot GPS-Denied Controller")
|
||||
print("=" * 50)
|
||||
print(f"Mode: {ardupilot.mode}")
|
||||
print(f"Armed: {ardupilot.armed}")
|
||||
print("")
|
||||
|
||||
if not args.no_takeoff:
|
||||
# Set GUIDED mode and arm
|
||||
print("[INFO] Setting up for takeoff...")
|
||||
ardupilot.set_mode("GUIDED")
|
||||
time.sleep(2) # Give mode time to change
|
||||
|
||||
# Update telemetry to get current mode
|
||||
ardupilot.update_telemetry()
|
||||
print(f"[INFO] Current mode: {ardupilot.mode}")
|
||||
|
||||
if not ardupilot.arm(force=True):
|
||||
print("[ERROR] Could not arm - continuing in manual mode")
|
||||
print("[INFO] Use MAVProxy to arm: arm throttle force")
|
||||
else:
|
||||
ardupilot.takeoff(args.takeoff_alt)
|
||||
print(f"[INFO] Taking off to {args.takeoff_alt}m...")
|
||||
|
||||
# Wait for takeoff (up to 15 seconds)
|
||||
for i in range(150):
|
||||
telemetry = ardupilot.get_telemetry()
|
||||
alt = telemetry["altimeter"]["altitude"]
|
||||
if alt > args.takeoff_alt * 0.9:
|
||||
print(f"\n[OK] Reached target altitude ({alt:.1f}m)")
|
||||
break
|
||||
if i % 10 == 0:
|
||||
print(f"\r[INFO] Climbing... {alt:.1f}m / {args.takeoff_alt}m", end="")
|
||||
time.sleep(0.1)
|
||||
print("")
|
||||
|
||||
print("\n[INFO] Starting controller loop...")
|
||||
print("[INFO] Press Ctrl+C to stop\n")
|
||||
|
||||
rate = 20 # Hz
|
||||
try:
|
||||
while True:
|
||||
# Get telemetry
|
||||
telemetry = ardupilot.get_telemetry()
|
||||
|
||||
# Run your controller
|
||||
thrust, pitch, roll, yaw = controller.calculate_landing_maneuver(
|
||||
telemetry,
|
||||
rover_telemetry=None # No rover in ArduPilot mode yet
|
||||
)
|
||||
|
||||
# Convert control outputs to velocities
|
||||
# thrust -> vertical velocity (negative = up)
|
||||
# pitch -> forward velocity
|
||||
# roll -> right velocity
|
||||
vz = -thrust * 2.0 # Scale factor
|
||||
vx = pitch * 2.0
|
||||
vy = roll * 2.0
|
||||
yaw_rate = yaw * 1.0
|
||||
|
||||
# Send velocity command
|
||||
ardupilot.send_velocity(vx, vy, vz, yaw_rate)
|
||||
|
||||
# Print status
|
||||
alt = telemetry["altimeter"]["altitude"]
|
||||
mode = telemetry.get("mode", "?")
|
||||
print(f"\rAlt: {alt:5.1f}m | Mode: {mode:10} | Cmd: vx={vx:+.2f} vy={vy:+.2f} vz={vz:+.2f}", end="")
|
||||
|
||||
time.sleep(1.0 / rate)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("\n\n[INFO] Stopping...")
|
||||
ardupilot.set_mode("LAND")
|
||||
print("[OK] Landing mode set")
|
||||
|
||||
# Import the simplified drone controller
|
||||
from src.drone_controller import main
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -1,12 +1,13 @@
|
||||
#!/bin/bash
|
||||
# =============================================================================
|
||||
# ArduPilot Controller Launcher
|
||||
# ArduPilot SITL + Controller Launcher
|
||||
# =============================================================================
|
||||
# Starts ArduPilot SITL and your drone controller together.
|
||||
# Starts SITL and the drone controller together.
|
||||
# Run Gazebo FIRST in another terminal!
|
||||
#
|
||||
# Usage:
|
||||
# Terminal 1: ./scripts/run_ardupilot_sim.sh runway
|
||||
# Terminal 2: ./scripts/run_ardupilot_controller.sh
|
||||
# Terminal 1: ./scripts/run_ardupilot_sim.sh
|
||||
# Terminal 2: ./scripts/run_ardupilot_controller.sh --pattern square
|
||||
# =============================================================================
|
||||
|
||||
set -e
|
||||
@@ -14,7 +15,7 @@ set -e
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
|
||||
|
||||
# Deactivate any existing virtual environment to avoid conflicts
|
||||
# Deactivate any existing venv
|
||||
if [ -n "$VIRTUAL_ENV" ]; then
|
||||
deactivate 2>/dev/null || true
|
||||
fi
|
||||
@@ -24,83 +25,72 @@ 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 " ArduPilot SITL + Controller"
|
||||
echo "=============================================="
|
||||
echo ""
|
||||
|
||||
# Check dependencies
|
||||
# Check sim_vehicle.py
|
||||
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"
|
||||
echo "Fix: source ~/.ardupilot_env"
|
||||
echo " Or: export PATH=\$PATH:~/ardupilot/Tools/autotest"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Verify empy is available
|
||||
# Check empy
|
||||
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"
|
||||
echo "Fix: pip install empy==3.3.4"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
echo "[INFO] Environment OK"
|
||||
echo "[INFO] Python: $(which python3)"
|
||||
echo "[OK] Environment ready"
|
||||
echo ""
|
||||
|
||||
# Start SITL with console output
|
||||
echo "[INFO] Starting ArduPilot SITL..."
|
||||
echo "[INFO] This will build ArduCopter if needed (first run takes ~2 min)"
|
||||
# Start SITL with console
|
||||
echo "[INFO] Starting SITL..."
|
||||
echo "[INFO] Make sure Gazebo is running first!"
|
||||
echo ""
|
||||
|
||||
# Run sim_vehicle in a way that shows output
|
||||
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map &
|
||||
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console &
|
||||
SITL_PID=$!
|
||||
|
||||
# Wait for SITL to be ready (check for TCP port 5760)
|
||||
echo "[INFO] Waiting for SITL to start (TCP 5760)..."
|
||||
# Wait for SITL to be ready
|
||||
echo "[INFO] Waiting for SITL (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"
|
||||
if [ $TRIES -ge 60 ]; then
|
||||
echo "[ERROR] SITL timeout"
|
||||
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"
|
||||
echo "[ERROR] SITL died - is Gazebo running?"
|
||||
exit 1
|
||||
fi
|
||||
echo -n "."
|
||||
done
|
||||
echo ""
|
||||
echo "[OK] SITL is ready on TCP 5760"
|
||||
echo "[OK] SITL ready"
|
||||
echo ""
|
||||
|
||||
# Give it a moment more to stabilize
|
||||
# Cleanup on exit
|
||||
trap "echo ''; echo '[INFO] Stopping...'; kill $SITL_PID 2>/dev/null; exit 0" INT TERM
|
||||
|
||||
# Wait a moment for SITL 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"
|
||||
# Run the controller
|
||||
echo "[INFO] Starting flight controller..."
|
||||
echo ""
|
||||
cd "$PROJECT_DIR"
|
||||
python scripts/run_ardupilot.py "$@"
|
||||
|
||||
@@ -1,96 +1,52 @@
|
||||
#!/bin/bash
|
||||
# =============================================================================
|
||||
# ArduPilot GPS-Denied Simulation Launcher
|
||||
# ArduPilot Gazebo Simulation Launcher
|
||||
# =============================================================================
|
||||
# Launches Gazebo with GPU acceleration and camera support
|
||||
# Automatically detects GPU: NVIDIA > Intel/AMD integrated > Software
|
||||
# Launches Gazebo with the ArduPilot iris drone model.
|
||||
# Start this FIRST, then run SITL in another terminal.
|
||||
#
|
||||
# Usage:
|
||||
# ./scripts/run_ardupilot_sim.sh # Default iris_runway
|
||||
# ./scripts/run_ardupilot_sim.sh camera # With camera world
|
||||
# ./scripts/run_ardupilot_sim.sh # Default runway
|
||||
# ./scripts/run_ardupilot_sim.sh warehouse # Indoor warehouse
|
||||
# =============================================================================
|
||||
|
||||
set -e
|
||||
|
||||
echo "=============================================="
|
||||
echo " ArduPilot GPS-Denied Simulation"
|
||||
echo " Gazebo + ArduPilot Simulation"
|
||||
echo "=============================================="
|
||||
echo ""
|
||||
|
||||
# Get script directory
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
|
||||
|
||||
# =============================================================================
|
||||
# GPU DETECTION & CONFIGURATION
|
||||
# GPU DETECTION
|
||||
# =============================================================================
|
||||
echo ""
|
||||
echo "[INFO] Detecting GPU..."
|
||||
|
||||
# Disable software rendering by default
|
||||
export LIBGL_ALWAYS_SOFTWARE=0
|
||||
|
||||
# Check for NVIDIA dedicated GPU
|
||||
if command -v nvidia-smi &> /dev/null && nvidia-smi &> /dev/null; then
|
||||
if command -v nvidia-smi &> /dev/null && nvidia-smi &> /dev/null 2>&1; then
|
||||
GPU_NAME=$(nvidia-smi --query-gpu=name --format=csv,noheader 2>/dev/null | head -1)
|
||||
echo "[GPU] NVIDIA (dedicated): $GPU_NAME"
|
||||
echo "[GPU] NVIDIA: $GPU_NAME"
|
||||
export __GLX_VENDOR_LIBRARY_NAME=nvidia
|
||||
export __NV_PRIME_RENDER_OFFLOAD=1
|
||||
export __VK_LAYER_NV_optimus=NVIDIA_only
|
||||
GPU_TYPE="nvidia"
|
||||
|
||||
# Check for Intel integrated GPU
|
||||
elif [ -d "/sys/class/drm/card0" ] && grep -qi "intel" /sys/class/drm/card0/device/vendor 2>/dev/null; then
|
||||
echo "[GPU] Intel (integrated)"
|
||||
export MESA_GL_VERSION_OVERRIDE=3.3
|
||||
export LIBVA_DRIVER_NAME=iHD # Intel media driver
|
||||
GPU_TYPE="intel"
|
||||
|
||||
# Check via glxinfo
|
||||
elif command -v glxinfo &> /dev/null; then
|
||||
GPU_NAME=$(glxinfo 2>/dev/null | grep "OpenGL renderer" | cut -d: -f2 | xargs)
|
||||
|
||||
if [[ "$GPU_NAME" == *"NVIDIA"* ]]; then
|
||||
echo "[GPU] NVIDIA: $GPU_NAME"
|
||||
export __GLX_VENDOR_LIBRARY_NAME=nvidia
|
||||
GPU_TYPE="nvidia"
|
||||
elif [[ "$GPU_NAME" == *"Intel"* ]]; then
|
||||
echo "[GPU] Intel: $GPU_NAME"
|
||||
export MESA_GL_VERSION_OVERRIDE=3.3
|
||||
GPU_TYPE="intel"
|
||||
elif [[ "$GPU_NAME" == *"AMD"* ]] || [[ "$GPU_NAME" == *"Radeon"* ]]; then
|
||||
echo "[GPU] AMD: $GPU_NAME"
|
||||
export MESA_GL_VERSION_OVERRIDE=3.3
|
||||
GPU_TYPE="amd"
|
||||
elif [[ "$GPU_NAME" == *"llvmpipe"* ]] || [[ "$GPU_NAME" == *"software"* ]]; then
|
||||
echo "[GPU] Software rendering (llvmpipe) - SLOW!"
|
||||
echo "[WARN] Install GPU drivers for better performance"
|
||||
GPU_TYPE="software"
|
||||
else
|
||||
echo "[GPU] $GPU_NAME"
|
||||
export MESA_GL_VERSION_OVERRIDE=3.3
|
||||
GPU_TYPE="other"
|
||||
fi
|
||||
|
||||
# Fallback to Mesa defaults
|
||||
echo "[GPU] $GPU_NAME"
|
||||
export MESA_GL_VERSION_OVERRIDE=3.3
|
||||
else
|
||||
echo "[GPU] Unknown - using Mesa defaults"
|
||||
export MESA_GL_VERSION_OVERRIDE=3.3
|
||||
GPU_TYPE="unknown"
|
||||
fi
|
||||
|
||||
# For integrated graphics, ensure Mesa works well
|
||||
if [[ "$GPU_TYPE" == "intel" ]] || [[ "$GPU_TYPE" == "amd" ]] || [[ "$GPU_TYPE" == "other" ]]; then
|
||||
export MESA_LOADER_DRIVER_OVERRIDE=""
|
||||
export DRI_PRIME=0 # Use primary GPU (integrated)
|
||||
fi
|
||||
|
||||
echo "[INFO] GPU type: $GPU_TYPE"
|
||||
|
||||
# =============================================================================
|
||||
# GAZEBO PATHS
|
||||
# =============================================================================
|
||||
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}"
|
||||
|
||||
# Add local models
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
|
||||
export GZ_SIM_RESOURCE_PATH="${PROJECT_DIR}/gazebo/models:${GZ_SIM_RESOURCE_PATH}"
|
||||
|
||||
# =============================================================================
|
||||
@@ -114,77 +70,48 @@ fi
|
||||
WORLD_ARG="${1:-runway}"
|
||||
|
||||
case "$WORLD_ARG" in
|
||||
runway|iris|default)
|
||||
runway|default)
|
||||
WORLD="${HOME}/ardupilot_gazebo/worlds/iris_runway.sdf"
|
||||
;;
|
||||
warehouse)
|
||||
WORLD="${HOME}/ardupilot_gazebo/worlds/iris_warehouse.sdf"
|
||||
;;
|
||||
gimbal)
|
||||
WORLD="${HOME}/ardupilot_gazebo/worlds/gimbal.sdf"
|
||||
;;
|
||||
zephyr|plane)
|
||||
WORLD="${HOME}/ardupilot_gazebo/worlds/zephyr_runway.sdf"
|
||||
;;
|
||||
parachute)
|
||||
WORLD="${HOME}/ardupilot_gazebo/worlds/zephyr_parachute.sdf"
|
||||
;;
|
||||
custom)
|
||||
WORLD="${PROJECT_DIR}/gazebo/worlds/custom_landing.sdf"
|
||||
;;
|
||||
*)
|
||||
if [ -f "$WORLD_ARG" ]; then
|
||||
WORLD="$WORLD_ARG"
|
||||
elif [ -f "${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}" ]; then
|
||||
WORLD="${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}"
|
||||
elif [ -f "${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}.sdf" ]; then
|
||||
WORLD="${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}.sdf"
|
||||
elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}" ]; then
|
||||
WORLD="${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}"
|
||||
elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf" ]; then
|
||||
WORLD="${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf"
|
||||
else
|
||||
WORLD=""
|
||||
echo "[ERROR] World not found: $WORLD_ARG"
|
||||
echo ""
|
||||
echo "Available worlds:"
|
||||
echo " runway - Outdoor runway (default)"
|
||||
echo " warehouse - Indoor warehouse"
|
||||
exit 1
|
||||
fi
|
||||
;;
|
||||
esac
|
||||
|
||||
if [ -z "$WORLD" ] || [ ! -f "$WORLD" ]; then
|
||||
echo "[ERROR] World not found: $WORLD_ARG"
|
||||
echo ""
|
||||
echo "Built-in worlds:"
|
||||
echo " runway - Iris on runway (default)"
|
||||
echo " warehouse - Iris in warehouse"
|
||||
echo " custom - Custom landing pad"
|
||||
echo ""
|
||||
echo "Local worlds in gazebo/worlds/:"
|
||||
ls -1 "${PROJECT_DIR}/gazebo/worlds/"*.sdf 2>/dev/null | xargs -n1 basename || echo " (none)"
|
||||
echo ""
|
||||
echo "Or specify full path to .sdf file"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
echo "[INFO] World: $(basename "$WORLD")"
|
||||
echo ""
|
||||
|
||||
# =============================================================================
|
||||
# INSTRUCTIONS
|
||||
# =============================================================================
|
||||
echo ""
|
||||
echo "=============================================="
|
||||
echo " Starting Gazebo"
|
||||
echo " STEP 1: Gazebo Starting..."
|
||||
echo "=============================================="
|
||||
echo ""
|
||||
echo "World: $WORLD"
|
||||
echo ""
|
||||
echo "After Gazebo starts, in another terminal run:"
|
||||
echo "After Gazebo loads, run SITL in another terminal:"
|
||||
echo ""
|
||||
echo " source ~/venv-ardupilot/bin/activate"
|
||||
echo " sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console"
|
||||
echo ""
|
||||
echo "For GPS-DENIED mode, in MAVProxy console:"
|
||||
echo "Then run the flight controller:"
|
||||
echo ""
|
||||
echo " param set ARMING_CHECK 0"
|
||||
echo " mode stabilize"
|
||||
echo " arm throttle force"
|
||||
echo " source ~/venv-ardupilot/bin/activate"
|
||||
echo " cd ~/RDC_Simulation"
|
||||
echo " python scripts/run_ardupilot.py --pattern square"
|
||||
echo ""
|
||||
echo "=============================================="
|
||||
echo ""
|
||||
|
||||
114
scripts/run_flight_demo.sh
Executable file
114
scripts/run_flight_demo.sh
Executable file
@@ -0,0 +1,114 @@
|
||||
#!/bin/bash
|
||||
# =============================================================================
|
||||
# ArduPilot Flight Demo with Camera Viewer
|
||||
# =============================================================================
|
||||
# Runs the drone controller and camera viewer together.
|
||||
#
|
||||
# Usage: ./scripts/run_flight_demo.sh [options]
|
||||
#
|
||||
# Options:
|
||||
# --pattern square|circle|hover Flight pattern (default: square)
|
||||
# --altitude HEIGHT Flight altitude in meters (default: 5)
|
||||
# --size SIZE Pattern size in meters (default: 5)
|
||||
# --no-camera Skip camera viewer
|
||||
# =============================================================================
|
||||
|
||||
set -e
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
|
||||
|
||||
# Parse arguments
|
||||
PATTERN="square"
|
||||
ALTITUDE="5"
|
||||
SIZE="5"
|
||||
SHOW_CAMERA=true
|
||||
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case $1 in
|
||||
--pattern|-p)
|
||||
PATTERN="$2"
|
||||
shift 2
|
||||
;;
|
||||
--altitude|-a)
|
||||
ALTITUDE="$2"
|
||||
shift 2
|
||||
;;
|
||||
--size|-s)
|
||||
SIZE="$2"
|
||||
shift 2
|
||||
;;
|
||||
--no-camera)
|
||||
SHOW_CAMERA=false
|
||||
shift
|
||||
;;
|
||||
*)
|
||||
shift
|
||||
;;
|
||||
esac
|
||||
done
|
||||
|
||||
# Deactivate any existing venv
|
||||
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
|
||||
|
||||
if [ -f "$HOME/venv-ardupilot/bin/activate" ]; then
|
||||
source "$HOME/venv-ardupilot/bin/activate"
|
||||
fi
|
||||
|
||||
echo "=============================================="
|
||||
echo " ArduPilot Flight Demo"
|
||||
echo "=============================================="
|
||||
echo ""
|
||||
echo " Pattern: $PATTERN"
|
||||
echo " Altitude: ${ALTITUDE}m"
|
||||
echo " Size: ${SIZE}m"
|
||||
echo " Camera: $SHOW_CAMERA"
|
||||
echo ""
|
||||
echo "=============================================="
|
||||
echo ""
|
||||
|
||||
# Check if ROS 2 is available for camera viewer
|
||||
ROS2_AVAILABLE=false
|
||||
if command -v ros2 &> /dev/null; then
|
||||
ROS2_AVAILABLE=true
|
||||
fi
|
||||
|
||||
# Start camera viewer in background if requested and ROS 2 is available
|
||||
CAMERA_PID=""
|
||||
if [ "$SHOW_CAMERA" = true ]; then
|
||||
if [ "$ROS2_AVAILABLE" = true ]; then
|
||||
echo "[INFO] Starting camera viewer..."
|
||||
source /opt/ros/humble/setup.bash 2>/dev/null || source /opt/ros/jazzy/setup.bash 2>/dev/null || true
|
||||
python "$PROJECT_DIR/src/camera_viewer.py" --topic /camera/image_raw &
|
||||
CAMERA_PID=$!
|
||||
sleep 2
|
||||
else
|
||||
echo "[WARN] ROS 2 not available - camera viewer disabled"
|
||||
echo "[INFO] To view camera, run Gazebo with camera in the world"
|
||||
fi
|
||||
fi
|
||||
|
||||
# Cleanup function
|
||||
cleanup() {
|
||||
echo ""
|
||||
echo "[INFO] Cleaning up..."
|
||||
if [ -n "$CAMERA_PID" ]; then
|
||||
kill $CAMERA_PID 2>/dev/null || true
|
||||
fi
|
||||
}
|
||||
trap cleanup EXIT INT TERM
|
||||
|
||||
# Run the flight controller
|
||||
echo "[INFO] Starting flight controller..."
|
||||
cd "$PROJECT_DIR"
|
||||
python scripts/run_ardupilot.py --pattern "$PATTERN" --altitude "$ALTITUDE" --size "$SIZE"
|
||||
|
||||
echo ""
|
||||
echo "[OK] Flight demo complete!"
|
||||
Reference in New Issue
Block a user