SirBlob/RDC_Simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduPilot SITL Update

Browse Source
This commit is contained in:
default
2026-01-04 00:24:46 +00:00
parent 6c72bbf24c
commit 6804180e21
20 changed files with 2138 additions and 2970 deletions
Download Patch File Download Diff File Expand all files Collapse all files

159
README.md
View File

@@ -1,154 +1,107 @@
# Drone Landing Simulation (GPS-Denied)
A GPS-denied drone landing simulation using relative sensors (IMU, altimeter, camera, landing pad detection) with multiple backends:
- **PyBullet** - Lightweight physics simulation
- **Gazebo** - Full robotics simulator
- **ArduPilot SITL** - Realistic flight controller with MAVProxy
A GPS-denied drone landing simulation with multiple backends. Land a drone on a moving platform using only relative sensors (IMU, altimeter, camera).
## Quick Start
### Standalone Mode (Any Platform - No ROS 2 Required)
### Standalone Mode (No ROS 2 - Any Platform)
```bash
source activate.sh # Linux/macOS
. .\activate.ps1 # Windows
source activate.sh
python standalone_simulation.py --pattern circular --speed 0.3
```
### PyBullet + ROS 2 (Two Terminals)
### Gazebo + ROS 2 (Linux/WSL2)
**Terminal 1 - Simulator:**
```bash
python simulation_host.py
```
**Terminal 2 - Bridge + Controllers:**
```bash
python run_bridge.py --pattern circular --speed 0.3
```
### Gazebo + ROS 2 (Two Terminals - Linux/WSL2)
**Terminal 1 - Launch Gazebo + Bridge:**
**Terminal 1:**
```bash
ros2 launch gazebo/launch/drone_landing.launch.py
```
**Terminal 2 - Run Controllers:**
**Terminal 2:**
```bash
python run_gazebo.py --pattern circular --speed 0.3
source activate.sh
python run_gazebo.py --pattern circular
python camera_viewer.py # Optional: view camera feed
```
### ArduPilot SITL + Gazebo (Three Terminals - Realistic Flight Controller)
### ArduPilot SITL (Realistic Flight Controller)
**Terminal 1 - Launch Gazebo with ArduPilot world:**
**Terminal 1:**
```bash
ros2 launch gazebo/launch/ardupilot_drone.launch.py
source ~/ardu_ws/install/setup.bash
ros2 launch ardupilot_gz_bringup iris_runway.launch.py
```
**Terminal 2 - Start ArduPilot SITL:**
**Terminal 2:**
```bash
cd ~/ardupilot
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
```
**Terminal 3 - Run MAVLink Bridge + Controllers:**
```bash
python run_ardupilot.py --no-sitl --pattern circular
mavproxy.py --console --map --master=:14550
```
## Installation
| Platform | Command |
|----------|---------|
```bash
# Ubuntu/Debian (includes ROS 2 + Gazebo)
./setup/install_ubuntu.sh
# With ArduPilot SITL
./setup/install_ardupilot.sh
# Activate environment
source activate.sh
```
| Platform | Install Script |
|----------|---------------|
| Ubuntu/Debian | `./setup/install_ubuntu.sh` |
| ArduPilot SITL | `./setup/install_ardupilot.sh` |
| Arch Linux | `./setup/install_arch.sh` |
| macOS | `./setup/install_macos.sh` |
| Windows | `.\setup\install_windows.ps1` |
## Platform Compatibility
| Feature | Ubuntu | Arch | macOS | Windows | WSL2 |
|---------|--------|------|-------|---------|------|
| Standalone | ✅ | ✅ | ✅ | ✅ | ✅ |
| PyBullet + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ |
| Gazebo + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ |
| ArduPilot + Gazebo | ✅ | ⚠️ | ❌ | ❌ | ✅ |
## Files
| File | Description |
|------|-------------|
| `standalone_simulation.py` | All-in-one (no ROS 2 required) |
| `simulation_host.py` | PyBullet simulator server |
| `run_bridge.py` | PyBullet bridge + Controllers |
| `run_gazebo.py` | Gazebo bridge + Controllers |
| `run_ardupilot.py` | **ArduPilot SITL** + MAVLink bridge |
| `mavlink_bridge.py` | MAVLink ↔ ROS 2 bridge |
| `config.py` | **Configuration file** (edit to customize) |
| `drone_controller.py` | **Your landing algorithm** (edit this!) |
| `rover_controller.py` | Moving landing pad controller |
| `standalone_simulation.py` | **All-in-one** (no ROS 2) |
| `run_gazebo.py` | Gazebo controllers |
| `run_ardupilot.py` | ArduPilot launcher |
| `camera_viewer.py` | Drone camera window |
| `drone_controller.py` | **Your landing algorithm** |
| `config.py` | Configuration |
## Sensors Available
| Sensor | Data |
|--------|------|
| IMU | Orientation (roll, pitch, yaw), angular velocity |
| Altimeter | Altitude, vertical velocity |
| Velocity | Estimated velocity (x, y, z) |
| Camera | Downward-facing image |
| Landing Pad | Relative position when visible |
## Configuration
Edit `config.py` to customize:
- Drone/rover starting positions
- Physical properties (mass, size)
- Drone/rover positions
- Controller gains (Kp, Kd)
- Landing detection thresholds
- Camera settings
- Landing thresholds
## Command Line Options
```bash
# Standalone (no ROS 2)
python standalone_simulation.py --pattern circular --speed 0.3
# PyBullet + ROS 2
python run_bridge.py --pattern circular --speed 0.3 --host <SIMULATOR_IP>
# Gazebo + ROS 2
python run_gazebo.py --pattern circular --speed 0.3
Options:
--pattern, -p stationary, linear, circular, square, random
--speed, -s Speed in m/s (default: 0.5)
--amplitude, -a Amplitude in meters (default: 2.0)
--host, -H Simulator IP (default: 0.0.0.0)
# Movement patterns
--pattern, -p stationary, linear, circular, square, random
--speed, -s Speed in m/s (default: 0.5)
--amplitude, -a Amplitude in meters (default: 2.0)
```
## GPS-Denied Sensors
| Sensor | Data |
|--------|------|
| **IMU** | Orientation (roll, pitch, yaw), angular velocity |
| **Altimeter** | Altitude, vertical velocity |
| **Velocity** | Estimated horizontal velocity (x, y, z) |
| **Camera** | 320x240 downward-facing JPEG image |
| **Landing Pad** | Relative position when visible in camera FOV |
## Documentation
| Document | Description |
|----------|-------------|
| [Installation](docs/installation.md) | Platform setup guides + WSL2 |
| [Architecture](docs/architecture.md) | System components diagram |
| [Gazebo Guide](docs/gazebo.md) | Gazebo-specific instructions |
| [PyBullet Guide](docs/pybullet.md) | PyBullet-specific instructions |
| [ArduPilot Guide](docs/ardupilot.md) | **ArduPilot SITL + MAVProxy** |
| [Protocol](docs/protocol.md) | Sensor data formats |
| [Drone Guide](docs/drone_guide.md) | Landing algorithm guide |
## Network Setup (Remote Simulator)
Run simulator on one machine, controllers on another:
**Machine 1 (with display):**
```bash
python simulation_host.py # Listens on 0.0.0.0:5555
```
**Machine 2 (headless):**
```bash
python run_bridge.py --host 192.168.1.100 # Connect to Machine 1
```
| [Installation](docs/installation.md) | Full setup guide |
| [Architecture](docs/architecture.md) | System overview |
| [ArduPilot](docs/ardupilot.md) | ArduPilot SITL guide |
| [Gazebo](docs/gazebo.md) | Gazebo guide |

103
build_exe.py
View File

@@ -7,6 +7,8 @@ Usage:
python build_exe.py # Build standalone_simulation
python build_exe.py simulation_host # Build simulation_host
python build_exe.py standalone # Build standalone_simulation
python build_exe.py ardupilot # Build ArduPilot launcher
python build_exe.py camera_viewer # Build camera feed viewer
python build_exe.py all # Build all
"""
@@ -24,12 +26,25 @@ except ImportError as e:
print("Install with: pip install pyinstaller pybullet")
sys.exit(1)
# Check for pymavlink (optional for ArduPilot builds)
try:
from pymavlink import mavutil
PYMAVLINK_AVAILABLE = True
except ImportError:
PYMAVLINK_AVAILABLE = False
def get_pybullet_data_path() -> str:
return pybullet_data.getDataPath()
def build_executable(source_name: str, output_name: str, console: bool = True):
def build_executable(
source_name: str,
output_name: str,
console: bool = True,
hidden_imports: list = None,
collect_data: list = None
):
"""Build a single executable."""
script_dir = Path(__file__).parent
source_file = script_dir / source_name
@@ -59,6 +74,16 @@ def build_executable(source_name: str, output_name: str, console: bool = True):
f'--add-data={data_spec}',
]
# Add hidden imports if specified
if hidden_imports:
for imp in hidden_imports:
build_args.append(f'--hidden-import={imp}')
# Add data collection for packages
if collect_data:
for pkg in collect_data:
build_args.append(f'--collect-data={pkg}')
if console:
build_args.append('--console')
else:
@@ -92,7 +117,7 @@ def main():
'target',
nargs='?',
default='standalone',
choices=['standalone', 'simulation_host', 'all'],
choices=['standalone', 'simulation_host', 'ardupilot', 'mavlink_bridge', 'camera_viewer', 'all'],
help='What to build (default: standalone)'
)
args = parser.parse_args()
@@ -102,9 +127,11 @@ def main():
print("=" * 60)
print(f"Platform: {platform.system()}")
print(f"PyBullet data: {get_pybullet_data_path()}")
print(f"pymavlink: {'Available' if PYMAVLINK_AVAILABLE else 'Not installed'}")
success = True
# Build standalone simulation
if args.target in ['standalone', 'all']:
success &= build_executable(
'standalone_simulation.py',
@@ -112,6 +139,7 @@ def main():
console=False
)
# Build simulation host
if args.target in ['simulation_host', 'all']:
success &= build_executable(
'simulation_host.py',
@@ -119,11 +147,81 @@ def main():
console=True
)
# Build MAVLink bridge (requires pymavlink)
if args.target in ['mavlink_bridge', 'all']:
if not PYMAVLINK_AVAILABLE:
print("\nWarning: pymavlink not installed, skipping mavlink_bridge build")
print("Install with: pip install pymavlink")
if args.target == 'mavlink_bridge':
success = False
else:
success &= build_executable(
'mavlink_bridge.py',
'mavlink_bridge',
console=True,
hidden_imports=[
'pymavlink',
'pymavlink.mavutil',
'pymavlink.dialects.v20.ardupilotmega',
],
collect_data=['pymavlink']
)
# Build ArduPilot runner (requires pymavlink)
if args.target in ['ardupilot', 'all']:
if not PYMAVLINK_AVAILABLE:
print("\nWarning: pymavlink not installed, skipping ardupilot build")
print("Install with: pip install pymavlink")
if args.target == 'ardupilot':
success = False
else:
success &= build_executable(
'run_ardupilot.py',
'run_ardupilot',
console=True,
hidden_imports=[
'pymavlink',
'pymavlink.mavutil',
'pymavlink.dialects.v20.ardupilotmega',
'mavlink_bridge',
'drone_controller',
'rover_controller',
],
collect_data=['pymavlink']
)
# Build camera viewer (requires opencv)
if args.target in ['camera_viewer', 'all']:
try:
import cv2
success &= build_executable(
'camera_viewer.py',
'camera_viewer',
console=True,
hidden_imports=[
'cv2',
'numpy',
],
collect_data=['cv2']
)
except ImportError:
print("\nWarning: opencv-python not installed, skipping camera_viewer build")
print("Install with: pip install opencv-python")
if args.target == 'camera_viewer':
success = False
print()
print("=" * 60)
if success:
print(" BUILD COMPLETE!")
print(" Executables in: dist/")
print()
print(" Available executables:")
dist_dir = Path(__file__).parent / "dist"
if dist_dir.exists():
for exe in dist_dir.iterdir():
if exe.is_file() and not exe.name.startswith('.'):
print(f" - {exe.name}")
else:
print(" BUILD FAILED!")
sys.exit(1)
@@ -132,3 +230,4 @@ def main():
if __name__ == '__main__':
main()

420
camera_viewer.py Normal file
View File

@@ -0,0 +1,420 @@
#!/usr/bin/env python3
"""
Drone Camera Viewer - Real-time camera feed display.
Displays the drone's camera feed from Gazebo simulation in a separate window.
Works with ROS 2 image topics.
Usage:
python camera_viewer.py # Default topic /drone/camera
python camera_viewer.py --topic /camera/image_raw
python camera_viewer.py --fps 60 # Higher framerate
python camera_viewer.py --record output.avi # Record to file
Topics:
- /drone/camera (Gazebo simulation)
- /camera/image_raw (generic)
- /ap/camera (ArduPilot if available)
"""
import argparse
import sys
import time
from typing import Optional
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from sensor_msgs.msg import Image
# Try to import OpenCV
try:
import cv2
import numpy as np
CV2_AVAILABLE = True
except ImportError:
CV2_AVAILABLE = False
print("Warning: OpenCV not installed. Install with: pip install opencv-python")
# Try to import cv_bridge (optional, for more robust conversion)
try:
from cv_bridge import CvBridge
CV_BRIDGE_AVAILABLE = True
except ImportError:
CV_BRIDGE_AVAILABLE = False
class CameraViewer(Node):
"""ROS 2 node that displays camera feed in a window."""
def __init__(
self,
topic: str = '/drone/camera',
fps: int = 30,
window_name: str = 'Drone Camera',
record_file: Optional[str] = None,
show_info: bool = True
):
super().__init__('camera_viewer')
self._topic = topic
self._target_fps = fps
self._window_name = window_name
self._show_info = show_info
self._record_file = record_file
# State
self._latest_frame: Optional[np.ndarray] = None
self._frame_count = 0
self._fps_actual = 0.0
self._last_fps_time = time.time()
self._fps_frame_count = 0
self._running = True
# Video writer for recording
self._video_writer: Optional[cv2.VideoWriter] = None
# CV Bridge for image conversion
if CV_BRIDGE_AVAILABLE:
self._bridge = CvBridge()
else:
self._bridge = None
# QoS for camera topics (best effort for performance)
camera_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1
)
# Subscribe to camera topic
self._camera_sub = self.create_subscription(
Image,
topic,
self._camera_callback,
camera_qos
)
self.get_logger().info('=' * 50)
self.get_logger().info('Camera Viewer Starting...')
self.get_logger().info(f' Topic: {topic}')
self.get_logger().info(f' Target FPS: {fps}')
if record_file:
self.get_logger().info(f' Recording to: {record_file}')
self.get_logger().info('=' * 50)
self.get_logger().info('Press Q or ESC in the window to quit')
# Create display timer at target FPS
display_period = 1.0 / fps
self._display_timer = self.create_timer(display_period, self._display_frame)
def _camera_callback(self, msg: Image):
"""Process incoming camera image."""
try:
# Convert ROS Image to OpenCV format
if self._bridge and CV_BRIDGE_AVAILABLE:
# Use cv_bridge for robust conversion
self._latest_frame = self._bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
else:
# Manual conversion
self._latest_frame = self._ros_image_to_cv2(msg)
self._frame_count += 1
self._fps_frame_count += 1
except Exception as e:
self.get_logger().warning(f'Failed to convert image: {e}')
def _ros_image_to_cv2(self, msg: Image) -> np.ndarray:
"""Convert ROS Image message to OpenCV format."""
# Get image dimensions
height = msg.height
width = msg.width
encoding = msg.encoding
# Convert based on encoding
if encoding in ['rgb8', 'bgr8']:
# 3-channel color image
channels = 3
dtype = np.uint8
elif encoding in ['rgba8', 'bgra8']:
# 4-channel color image
channels = 4
dtype = np.uint8
elif encoding in ['mono8', '8UC1']:
# Grayscale
channels = 1
dtype = np.uint8
elif encoding in ['mono16', '16UC1']:
channels = 1
dtype = np.uint16
elif encoding == '32FC1':
channels = 1
dtype = np.float32
else:
# Try to handle as 3-channel uint8
channels = 3
dtype = np.uint8
# Reshape data to image
if channels == 1:
frame = np.frombuffer(msg.data, dtype=dtype).reshape((height, width))
# Convert grayscale to BGR for display
frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
else:
frame = np.frombuffer(msg.data, dtype=dtype).reshape((height, width, channels))
# Convert to BGR if needed
if encoding == 'rgb8':
frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
elif encoding == 'rgba8':
frame = cv2.cvtColor(frame, cv2.COLOR_RGBA2BGR)
elif encoding == 'bgra8':
frame = cv2.cvtColor(frame, cv2.COLOR_BGRA2BGR)
return frame
def _display_frame(self):
"""Display the latest frame."""
if not self._running:
return
# Calculate actual FPS
current_time = time.time()
elapsed = current_time - self._last_fps_time
if elapsed >= 1.0:
self._fps_actual = self._fps_frame_count / elapsed
self._fps_frame_count = 0
self._last_fps_time = current_time
# Check for window close or key press
key = cv2.waitKey(1) & 0xFF
if key in [ord('q'), ord('Q'), 27]: # Q or ESC
self._running = False
self.get_logger().info('Shutting down...')
rclpy.shutdown()
return
# Display frame if available
if self._latest_frame is not None:
display_frame = self._latest_frame.copy()
# Add info overlay
if self._show_info:
self._add_info_overlay(display_frame)
# Show frame
cv2.imshow(self._window_name, display_frame)
# Record if enabled
if self._record_file and self._video_writer is None:
# Initialize video writer with frame dimensions
h, w = display_frame.shape[:2]
fourcc = cv2.VideoWriter_fourcc(*'XVID')
self._video_writer = cv2.VideoWriter(
self._record_file, fourcc, self._target_fps, (w, h)
)
if self._video_writer:
self._video_writer.write(display_frame)
else:
# No frame yet - show waiting message
waiting_frame = np.zeros((480, 640, 3), dtype=np.uint8)
cv2.putText(
waiting_frame,
'Waiting for camera feed...',
(120, 240),
cv2.FONT_HERSHEY_SIMPLEX,
1.0,
(255, 255, 255),
2
)
cv2.putText(
waiting_frame,
f'Topic: {self._topic}',
(100, 280),
cv2.FONT_HERSHEY_SIMPLEX,
0.7,
(128, 128, 128),
1
)
cv2.imshow(self._window_name, waiting_frame)
def _add_info_overlay(self, frame: np.ndarray):
"""Add information overlay to frame."""
h, w = frame.shape[:2]
# Semi-transparent background for text
overlay = frame.copy()
cv2.rectangle(overlay, (5, 5), (200, 85), (0, 0, 0), -1)
cv2.addWeighted(overlay, 0.5, frame, 0.5, 0, frame)
# FPS display
cv2.putText(
frame,
f'FPS: {self._fps_actual:.1f}',
(10, 25),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
(0, 255, 0),
2
)
# Frame count
cv2.putText(
frame,
f'Frame: {self._frame_count}',
(10, 50),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
(0, 255, 0),
2
)
# Resolution
cv2.putText(
frame,
f'Size: {w}x{h}',
(10, 75),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
(0, 255, 0),
2
)
# Recording indicator
if self._record_file:
cv2.circle(frame, (w - 20, 20), 10, (0, 0, 255), -1)
cv2.putText(
frame,
'REC',
(w - 60, 25),
cv2.FONT_HERSHEY_SIMPLEX,
0.5,
(0, 0, 255),
2
)
def shutdown(self):
"""Clean up resources."""
self._running = False
if self._video_writer:
self._video_writer.release()
cv2.destroyAllWindows()
def find_camera_topics() -> list:
"""Find available camera topics."""
import subprocess
try:
result = subprocess.run(
['ros2', 'topic', 'list'],
capture_output=True,
text=True,
timeout=5.0
)
topics = result.stdout.strip().split('\n')
# Filter for likely camera topics
camera_keywords = ['camera', 'image', 'rgb', 'depth']
camera_topics = [
t for t in topics
if any(kw in t.lower() for kw in camera_keywords)
]
return camera_topics
except Exception:
return []
def main():
if not CV2_AVAILABLE:
print("ERROR: OpenCV is required for camera viewer")
print("Install with: pip install opencv-python")
sys.exit(1)
parser = argparse.ArgumentParser(
description='Drone Camera Viewer - Display camera feed in real-time',
formatter_class=argparse.RawDescriptionHelpFormatter,
epilog="""
Examples:
python camera_viewer.py # Default topic
python camera_viewer.py --topic /camera/image_raw
python camera_viewer.py --fps 60 # Higher framerate
python camera_viewer.py --record flight.avi # Record video
python camera_viewer.py --list # List camera topics
Common topics:
/drone/camera - Gazebo simulation
/camera/image_raw - Generic camera
/camera/color/image_raw - RGB camera
/camera/depth/image_raw - Depth camera
"""
)
parser.add_argument(
'--topic', '-t', type=str, default='/drone/camera',
help='Camera image topic (default: /drone/camera)'
)
parser.add_argument(
'--fps', type=int, default=30,
help='Target display framerate (default: 30)'
)
parser.add_argument(
'--record', '-r', type=str, default=None,
help='Record video to file (e.g., output.avi)'
)
parser.add_argument(
'--no-info', action='store_true',
help='Hide info overlay'
)
parser.add_argument(
'--list', '-l', action='store_true',
help='List available camera topics and exit'
)
parser.add_argument(
'--window-name', '-w', type=str, default='Drone Camera',
help='Window title'
)
args = parser.parse_args()
# List topics and exit
if args.list:
print("Searching for camera topics...")
topics = find_camera_topics()
if topics:
print("\nAvailable camera topics:")
for t in topics:
print(f" {t}")
else:
print("\nNo camera topics found. Is the simulation running?")
return
# Initialize ROS 2
rclpy.init()
viewer = None
try:
viewer = CameraViewer(
topic=args.topic,
fps=args.fps,
window_name=args.window_name,
record_file=args.record,
show_info=not args.no_info
)
rclpy.spin(viewer)
except KeyboardInterrupt:
print('\nShutting down...')
finally:
if viewer:
viewer.shutdown()
viewer.destroy_node()
if rclpy.ok():
rclpy.shutdown()
cv2.destroyAllWindows()
if __name__ == '__main__':
main()

61
config.py
View File

@@ -47,12 +47,19 @@ ROVER = {
# =============================================================================
CAMERA = {
# Camera sensor settings (for simulation)
"width": 320,
"height": 240,
"fov": 60.0, # Field of view in degrees
"near_clip": 0.1, # Near clipping plane
"far_clip": 100.0, # Far clipping plane
"jpeg_quality": 70, # JPEG compression quality (1-100)
# Camera viewer settings (for camera_viewer.py)
"viewer_fps": 30, # Display framerate
"viewer_topic": "/drone/camera", # ROS 2 topic for Gazebo
"viewer_topic_ardupilot": "/camera/image_raw", # Topic for ArduPilot
"show_info_overlay": True, # Show FPS and frame count
}
# =============================================================================
@@ -103,47 +110,57 @@ CONTROLLER = {
}
# =============================================================================
# ARDUPILOT / MAVLINK CONFIGURATION
# ARDUPILOT ROS 2 CONFIGURATION
# =============================================================================
ARDUPILOT = {
# Vehicle type
"vehicle": "ArduCopter", # ArduCopter, ArduPlane, APMrover2
"frame": "gazebo-iris", # Gazebo model frame
"frame": "iris", # Gazebo model (iris, wildthumper)
# SITL connection
"sitl_host": "127.0.0.1", # SITL host address
"sitl_port": 5760, # SITL TCP port
# ROS 2 workspace (set by install_ardupilot.sh)
"workspace": "~/ardu_ws",
# MAVProxy output (for MAVLink bridge)
"mavproxy_host": "127.0.0.1",
"mavproxy_port": 14550, # MAVProxy UDP output
# Simulation worlds
"default_world": "runway", # runway, maze, sitl
# Gazebo plugin connection (JSON interface)
"gazebo_fdm_port_in": 9002, # Port for motor commands
"gazebo_fdm_port_out": 9003, # Port for sensor data
# MAVProxy connection (for GCS features)
"mavproxy_port": 14550, # MAVProxy UDP port
# Arming requirements (for simulation, can be relaxed)
"require_gps": False, # GPS required for arming
"require_ekf": True, # EKF required for arming
# DDS configuration
"dds_enable": True, # Enable DDS for native ROS 2 topics
"dds_domain_id": 0, # Must match ROS_DOMAIN_ID
}
# ArduPilot ROS 2 Topics (read-only reference)
ARDUPILOT_TOPICS = {
# State topics (subscribe)
"pose": "/ap/pose/filtered", # PoseStamped
"geopose": "/ap/geopose/filtered", # GeoPoseStamped
"twist": "/ap/twist/filtered", # TwistStamped
"imu": "/ap/imu/filtered", # Imu
"battery": "/ap/battery", # BatteryState
"navsat": "/ap/navsat", # NavSatFix
# Command topics (publish) - via MAVLink
"cmd_vel": "/cmd_vel", # Twist (velocity commands)
}
# MAVLink configuration (for pymavlink/mavproxy)
MAVLINK = {
# MAVLink system IDs
"system_id": 1, # Our system ID
"component_id": 191, # MAV_COMP_ID_MISSIONPLANNER
# Target system (usually the autopilot)
# Target system (the autopilot)
"target_system": 1,
"target_component": 1,
# Connection timeout (seconds)
# Connection
"connection_string": "udpin:127.0.0.1:14550",
# Timeouts (seconds)
"heartbeat_timeout": 5.0,
"connection_timeout": 30.0,
# Data stream rates (Hz)
"stream_rate_position": 50,
"stream_rate_attitude": 50,
"stream_rate_raw_sensors": 50,
"stream_rate_extended_status": 5,
}

206
docs/architecture.md
View File

@@ -1,176 +1,94 @@
# Architecture Overview
GPS-denied drone landing simulation with multiple operation modes.
## Operation Modes
### 1. Standalone Mode (Any Platform)
### 1. Standalone (Any Platform)
Single-process simulation - no ROS 2 or networking required:
Single process, no ROS 2 required:
```bash
python standalone_simulation.py --pattern circular
```
```
┌────────────────────────────────────────┐
│ standalone_simulation.py │
│ ┌──────────────────────────────────┐ │
│ │ PyBullet Physics + Camera │ │
│ │ Built-in Landing Controller │ │
│ │ Rover Movement Patterns │ │
│ │ Configuration from config.py │ │
│ │ Built-in Controller │ │
│ │ Rover Movement │ │
│ └──────────────────────────────────┘ │
└────────────────────────────────────────┘
```
### 2. PyBullet + ROS 2 Mode (Two Terminals)
### 2. Gazebo + ROS 2 (2 Terminals)
**Terminal 1:**
```bash
ros2 launch gazebo/launch/drone_landing.launch.py
```
**Terminal 2:**
```bash
python run_gazebo.py --pattern circular
```
```
Terminal 1 Terminal 2
┌──────────────────┐ ┌──────────────────────────┐
│ simulation_host │◄─UDP───►│ run_bridge.py │
│ (PyBullet) │ │ ┌────────────────────┐ │
│ Port 5555 │ │ │ ROS2SimulatorBridge│ │
│ │ │ │ DroneController │ │
│ │ │ │ RoverController │ │
└──────────────────┘ │ └────────────────────┘ │
└──────────────────────────┘
┌───────────────────┐ ┌───────────────────┐
│ Gazebo + Bridge │◄──────►│ run_gazebo.py │
│ (Physics) │ ROS │ + Controllers │
└───────────────────┘ └───────────────────┘
```
Data flow:
- RoverController publishes position → Bridge sends to Simulator
- Simulator moves rover visually AND sends back telemetry
- DroneController receives telemetry, publishes commands
- Bridge forwards commands to Simulator
### 3. ArduPilot SITL (2 Terminals)
### 3. Gazebo + ROS 2 Mode (Two Terminals, Linux/WSL2)
```
Terminal 1 Terminal 2
┌───────────────────────────┐ ┌──────────────────────────┐
│ ros2 launch ... .launch.py│ │ run_gazebo.py │
│ ┌─────────────────────┐ │ │ ┌────────────────────┐ │
│ │ Gazebo (ign gazebo) │ │ │ │ GazeboBridge │ │
│ │ - Drone (vel ctrl) │ │◄────►│ │ DroneController │ │
│ │ - Rover (vel ctrl) │ │ ROS │ │ RoverController │ │
│ ├─────────────────────┤ │ │ └────────────────────┘ │
│ │ ros_gz_bridge │ │ └──────────────────────────┘
│ └─────────────────────┘ │
└───────────────────────────┘
**Terminal 1:**
```bash
ros2 launch ardupilot_gz_bringup iris_runway.launch.py
```
Data flow:
- RoverController publishes to `/rover/cmd_vel` → Gazebo moves rover
- Gazebo publishes odometry → GazeboBridge converts to telemetry
- DroneController receives telemetry, publishes to `/cmd_vel`
- GazeboBridge forwards to `/drone/cmd_vel` → Gazebo moves drone
### 4. ArduPilot SITL + Gazebo Mode (Three Terminals, Linux/WSL2)
```
Terminal 1 Terminal 2 Terminal 3
┌──────────────┐ ┌─────────────────┐ ┌────────────────────────┐
│ Gazebo + │ │ ArduPilot SITL │ │ run_ardupilot.py │
│ ArduPilot │◄──►│ sim_vehicle.py │ │ ┌──────────────────┐ │
│ Plugin │JSON│ + MAVProxy │◄───►│ │ MAVLinkBridge │ │
│ │ │ │ UDP │ │ DroneController │ │
│ ardupilot_ │ │ Flight Control │ │ │ RoverController │ │
│ drone.sdf │ │ + GCS │ │ └──────────────────┘ │
└──────────────┘ └─────────────────┘ └────────────────────────┘
**Terminal 2:**
```bash
mavproxy.py --console --map --master=:14550
```
Data flow:
- ArduPilot SITL sends motor commands → Gazebo plugin controls drone
- Gazebo plugin sends sensor data → ArduPilot SITL for state estimation
- MAVProxy outputs telemetry → MAVLinkBridge converts to ROS telemetry
- DroneController receives telemetry, publishes velocity commands
- MAVLinkBridge sends MAVLink commands → ArduPilot SITL executes
```
┌─────────────────────────────────────────────┐
│ Single Launch Command │
│ (Starts SITL + Gazebo + RViz) │
├─────────────────────────────────────────────┤
│ ArduPilot SITL ◄──► Gazebo ◄──► ROS 2 │
│ ▲ │
│ │ /ap/* topics │
│ ▼ │
│ MAVProxy (GCS) │
└─────────────────────────────────────────────┘
```
Key differences from simple Gazebo mode:
- Full ArduPilot flight controller (EKF, stabilization, failsafes)
- Real MAVLink protocol for commands and telemetry
- Support for all ArduPilot flight modes (GUIDED, LAND, etc.)
- Arming checks and safety features
- Compatible with ground control stations (QGroundControl, Mission Planner)
## Key Components
## Components
| File | Description |
|------|-------------|
| `config.py` | Central configuration (positions, physics, gains) |
| `standalone_simulation.py` | All-in-one simulation |
| `simulation_host.py` | PyBullet physics server (UDP) |
| `run_bridge.py` | PyBullet bridge + controllers |
| `run_gazebo.py` | Gazebo bridge + controllers |
| `run_ardupilot.py` | **ArduPilot SITL** + MAVLink bridge |
| `mavlink_bridge.py` | MAVLink ↔ ROS 2 bridge |
| `drone_controller.py` | **Your landing algorithm** |
| `rover_controller.py` | Moving landing pad |
| `ros_bridge.py` | ROS-UDP bridge (used by run_bridge.py) |
| `gazebo_bridge.py` | Gazebo-ROS bridge (used by run_gazebo.py) |
| `gazebo/launch/drone_landing.launch.py` | ROS 2 launch file for Gazebo |
| `gazebo/launch/ardupilot_drone.launch.py` | ROS 2 launch file for ArduPilot |
| `gazebo/worlds/drone_landing.sdf` | Gazebo world with simple velocity control |
| `gazebo/worlds/ardupilot_drone.sdf` | Gazebo world with ArduPilot plugin |
| Component | Description |
|-----------|-------------|
| `drone_controller.py` | Your landing algorithm |
| `gazebo_bridge.py` | Gazebo ↔ ROS bridge |
| `mavlink_bridge.py` | MAVLink commands |
| `camera_viewer.py` | Camera display |
## ROS 2 Topics
| Topic | Type | Description |
|-------|------|-------------|
| `/cmd_vel` | `Twist` | Drone commands from DroneController |
| `/drone/cmd_vel` | `Twist` | Drone commands to Gazebo |
| `/drone/telemetry` | `String` | GPS-denied sensor data (JSON) |
| `/rover/cmd_vel` | `Twist` | Rover velocity to simulator |
| `/rover/telemetry` | `String` | Rover position (JSON) |
| Topic | Direction | Description |
|-------|-----------|-------------|
| `/drone/telemetry` | ← | Sensor data (JSON) |
| `/cmd_vel` | → | Velocity commands |
| `/drone/camera` | ← | Camera images |
| `/rover/telemetry` | ← | Landing pad position |
## Network Configuration
## ArduPilot Topics
All components default to `0.0.0.0` for network accessibility.
### Remote Setup (PyBullet mode)
**Machine 1 (with display):**
```bash
python simulation_host.py # Listens on 0.0.0.0:5555
```
**Machine 2 (headless controller):**
```bash
python run_bridge.py --host 192.168.1.100
```
### UDP Ports
| Port | Direction | Content |
|------|-----------|---------|
| 5555 | Bridge → Simulator | Commands (JSON) |
| 5556 | Simulator → Bridge | Telemetry (JSON) |
## GPS-Denied Sensors
All modes provide the same sensor data:
| Sensor | Data |
|--------|------|
| **IMU** | Orientation (roll, pitch, yaw), angular velocity |
| **Altimeter** | Altitude above ground, vertical velocity |
| **Velocity** | Estimated velocity (x, y, z) |
| **Camera** | 320x240 downward JPEG (base64) |
| **Landing Pad** | Relative position (x, y, distance) when visible |
## Configuration (config.py)
| Section | Parameters |
|---------|------------|
| `DRONE` | mass, size, color, start_position, thrust/torque scales |
| `ROVER` | size, color, start_position, default_pattern, default_speed |
| `CAMERA` | width, height, fov, jpeg_quality |
| `PHYSICS` | gravity, timestep, telemetry_rate |
| `CONTROLLER` | Kp_z, Kd_z, Kp_xy, Kd_xy, rate |
| `LANDING` | success_distance, success_velocity, height_threshold |
| `NETWORK` | host, command_port, telemetry_port |
## Platform Support
| Mode | Ubuntu | Arch | macOS | Windows | WSL2 |
|------|--------|------|-------|---------|------|
| Standalone | ✅ | ✅ | ✅ | ✅ | ✅ |
| PyBullet+ROS | ✅ | ⚠️ | ❌ | ❌ | ✅ |
| Gazebo+ROS | ✅ | ⚠️ | ❌ | ❌ | ✅ |
| Topic | Type |
|-------|------|
| `/ap/pose/filtered` | Position |
| `/ap/twist/filtered` | Velocity |
| `/ap/imu/filtered` | IMU |
| `/ap/battery` | Battery |

286
docs/ardupilot.md
View File

@@ -1,285 +1,101 @@
# ArduPilot SITL + Gazebo Integration
# ArduPilot SITL Integration
This guide explains how to run the drone simulation with ArduPilot Software-In-The-Loop (SITL) and MAVProxy, providing a realistic flight controller stack.
## Overview
The ArduPilot integration replaces the simple velocity control with a full ArduPilot flight stack:
```
┌──────────────────┐ ┌─────────────────┐ ┌──────────────────┐
│ ArduPilot SITL │◄───►│ Gazebo + Plugin │◄───►│ MAVLink Bridge │
│ (Flight Control)│ JSON│ (Physics Sim) │ ROS │ + Controllers │
└──────────────────┘ └─────────────────┘ └──────────────────┘
▲ │
│ UDP │
│ ▼
┌──────────────────┐ ┌──────────────────┐
│ MAVProxy │◄────────────────────────────►│ DroneController │
│ (GCS) │ MAVLink Commands │ (Your Algorithm) │
└──────────────────┘ └──────────────────┘
```
## Components
| Component | Description |
|-----------|-------------|
| **ArduPilot SITL** | Full autopilot firmware running in simulation |
| **ardupilot_gazebo** | Plugin connecting Gazebo physics to ArduPilot |
| **MAVProxy** | Ground Control Station for monitoring/commands |
| **MAVLink Bridge** | ROS 2 node bridging MAVLink ↔ ROS topics |
| **Drone Controller** | Your landing algorithm |
## Prerequisites
### 1. ArduPilot SITL
Install ArduPilot development environment:
```bash
# Ubuntu/Debian
git clone https://github.com/ArduPilot/ardupilot.git ~/ardupilot
cd ~/ardupilot
git submodule update --init --recursive
Tools/environment_install/install-prereqs-ubuntu.sh -y
. ~/.profile
# Set environment
echo 'export PATH=$PATH:$HOME/ardupilot/Tools/autotest' >> ~/.bashrc
echo 'export ARDUPILOT_HOME=$HOME/ardupilot' >> ~/.bashrc
source ~/.bashrc
```
### 2. ArduPilot Gazebo Plugin
Install the ardupilot_gazebo plugin:
```bash
# For Gazebo Garden/Harmonic
git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo
cd ~/ardupilot_gazebo
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j4
# Add to Gazebo plugin path
echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH' >> ~/.bashrc
echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH' >> ~/.bashrc
source ~/.bashrc
```
### 3. pymavlink
```bash
pip install pymavlink
```
Run the simulation with a realistic ArduPilot flight controller.
## Quick Start
### Option 1: Integrated Launch (Recommended)
This starts everything together:
**Terminal 1 - Simulation:**
```bash
# Terminal 1: Start Gazebo
ros2 launch gazebo/launch/ardupilot_drone.launch.py
# Terminal 2: Start SITL
cd ~/ardupilot
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map
# Terminal 3: Run bridge + controllers
python run_ardupilot.py --no-sitl --pattern circular
source ~/ardu_ws/install/setup.bash
ros2 launch ardupilot_gz_bringup iris_runway.launch.py
```
### Option 2: Manual Setup
**Terminal 2 - Control:**
```bash
# Terminal 1: Start Gazebo world
gz sim -r gazebo/worlds/ardupilot_drone.sdf
# Terminal 2: Start ArduPilot SITL
cd ~/ardupilot
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
# Terminal 3: Run MAVLink bridge + controllers
python run_ardupilot.py --no-sitl
mavproxy.py --console --map --master=:14550
```
### Option 3: Full Automatic
## Installation
```bash
# Starts everything (requires SITL installed)
python run_ardupilot.py --pattern circular --console --map
./setup/install_ardupilot.sh
source ~/.bashrc
```
## Flight Operations
This installs:
- ArduPilot SITL with DDS
- Gazebo with ardupilot_gz
- MAVProxy
### Using MAVProxy Commands
Once connected, use MAVProxy to control the drone:
## MAVProxy Commands
```bash
# Set GUIDED mode for algorithm control
# Set mode
mode guided
# Arm motors
# Arm
arm throttle
# Take off to 5 meters
# Takeoff
takeoff 5
# Land
mode land
# Disarm
disarm
```
### Using the MAVLink Bridge API
## ROS 2 Topics
From Python, you can control the drone directly:
ArduPilot publishes native ROS 2 topics:
```python
from mavlink_bridge import MAVLinkBridge
```bash
# List topics
ros2 topic list
# Create bridge
bridge = MAVLinkBridge(sitl_port=14550)
# View position
ros2 topic echo /ap/geopose/filtered
# Arm and takeoff
bridge.set_mode('GUIDED')
bridge.arm()
bridge.takeoff(altitude=5.0)
# Land
bridge.land()
# View battery
ros2 topic echo /ap/battery
```
## Files
| Topic | Type |
|-------|------|
| `/ap/pose/filtered` | PoseStamped |
| `/ap/twist/filtered` | TwistStamped |
| `/ap/imu/filtered` | Imu |
| `/ap/battery` | BatteryState |
| File | Description |
|------|-------------|
| `mavlink_bridge.py` | ROS 2 ↔ MAVLink bridge |
| `run_ardupilot.py` | Integrated launcher |
| `gazebo/worlds/ardupilot_drone.sdf` | Gazebo world with ArduPilot plugin |
| `gazebo/launch/ardupilot_drone.launch.py` | ROS 2 launch file |
## Available Worlds
## Configuration
```bash
# Iris on runway
ros2 launch ardupilot_gz_bringup iris_runway.launch.py
Edit `config.py` to adjust ArduPilot settings:
# Iris in maze
ros2 launch ardupilot_gz_bringup iris_maze.launch.py
```python
ARDUPILOT = {
"vehicle": "ArduCopter", # ArduCopter, ArduPlane, APMrover2
"frame": "gazebo-iris", # Gazebo frame
"sitl_host": "127.0.0.1",
"sitl_port": 5760,
"mavproxy_port": 14550,
}
MAVLINK = {
"system_id": 1,
"component_id": 191,
"heartbeat_timeout": 5.0,
}
# Rover
ros2 launch ardupilot_gz_bringup wildthumper_playpen.launch.py
```
## Telemetry Format
## Using the Launcher
The MAVLink bridge publishes telemetry in the same format as other modes:
```json
{
"imu": {
"orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0},
"angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}
},
"altimeter": {
"altitude": 5.0,
"vertical_velocity": 0.0
},
"velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
"position": {"x": 0.0, "y": 0.0, "z": 5.0},
"landing_pad": {
"relative_x": 0.5,
"relative_y": 0.2,
"distance": 4.8,
"confidence": 0.95
},
"battery": {"voltage": 12.6, "remaining": 100},
"armed": true,
"flight_mode": "GUIDED",
"connected": true
}
```bash
python run_ardupilot.py --world runway
python run_ardupilot.py --world maze
python run_ardupilot.py --vehicle rover
```
## Troubleshooting
### SITL Not Starting
**No ROS 2 topics:**
```bash
# Check if SITL is installed
which sim_vehicle.py
# Set ArduPilot path
export ARDUPILOT_HOME=~/ardupilot
export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest
# Check DDS is enabled
param set DDS_ENABLE 1
```
### Gazebo Plugin Not Found
**Can't arm:**
```bash
# Check plugin path
echo $GZ_SIM_SYSTEM_PLUGIN_PATH
# Verify plugin exists
ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
```
### No MAVLink Connection
```bash
# Check if SITL is listening
netstat -tuln | grep 14550
# Test with mavlink console
python -c "from pymavlink import mavutil; c = mavutil.mavlink_connection('udpin:127.0.0.1:14550'); print(c.wait_heartbeat())"
```
### Drone Won't Arm
Common issues:
1. **Pre-arm checks failing** - Check MAVProxy console for errors
2. **GPS required** - In simulation, you may need to wait for GPS lock
3. **EKF not ready** - Wait for EKF to initialize
Disable pre-arm checks for testing (not recommended for real flights):
```
# In MAVProxy
# Disable pre-arm checks (simulation only)
param set ARMING_CHECK 0
```
## Flight Modes
| Mode | Description |
|------|-------------|
| **GUIDED** | Accept velocity/position commands from controller |
| **LOITER** | Hold position (GPS required) |
| **ALT_HOLD** | Maintain altitude, manual horizontal |
| **LAND** | Automatic landing |
| **STABILIZE** | Attitude stabilization only |
For autonomous landing, use **GUIDED** mode.
## Architecture Comparison
| Feature | Simple Gazebo | ArduPilot + Gazebo |
|---------|--------------|-------------------|
| Flight Controller | Velocity control | Full ArduPilot |
| Stabilization | Manual PD | Inbuilt EKF + PID |
| Flight Modes | None | All ArduPilot modes |
| Arming | Not required | Safety checks |
| Failsafes | None | Battery, GPS, etc. |
| MAVLink | No | Full protocol |
| GCS Support | No | QGC, Mission Planner |
| Realism | Low | High |

155
docs/drone_guide.md
View File

@@ -1,28 +1,23 @@
# DroneController Guide (GPS-Denied)
# DroneController Guide
Implement your landing algorithm in `drone_controller.py`.
Implement your GPS-denied landing algorithm in `drone_controller.py`.
## Quick Start
1. Edit `drone_controller.py`
2. Find `calculate_landing_maneuver()`
3. Implement your algorithm
4. Test with any mode:
- `python standalone_simulation.py --pattern stationary` (standalone)
- `python run_bridge.py --pattern stationary` (PyBullet + ROS 2)
- `python run_gazebo.py --pattern stationary` (Gazebo + ROS 2)
4. Test: `python standalone_simulation.py`
## GPS-Denied Challenge
## Sensors Available
No GPS available. You must use:
| Sensor | Data |
|--------|------|
| **IMU** | Orientation, angular velocity |
| **Altimeter** | Altitude, vertical velocity |
| **Velocity** | Estimated from optical flow |
| **Camera** | 320x240 downward image (base64 JPEG) |
| **Landing Pad** | Relative position (may be null!) |
| Sensor | Description |
|--------|-------------|
| IMU | Orientation, angular velocity |
| Altimeter | Altitude, vertical velocity |
| Velocity | Estimated velocity (x, y, z) |
| Camera | 320x240 downward image |
| Landing Pad | Relative position (may be null!) |
## Function to Implement
@@ -34,55 +29,20 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry):
## Sensor Data
### IMU
```python
imu = telemetry['imu']
roll = imu['orientation']['roll']
pitch = imu['orientation']['pitch']
yaw = imu['orientation']['yaw']
angular_vel = imu['angular_velocity'] # {x, y, z}
```
# Altitude
altitude = telemetry['altimeter']['altitude']
vertical_vel = telemetry['altimeter']['vertical_velocity']
### Altimeter
```python
altimeter = telemetry['altimeter']
altitude = altimeter['altitude']
vertical_vel = altimeter['vertical_velocity']
```
# Velocity
vel_x = telemetry['velocity']['x']
vel_y = telemetry['velocity']['y']
### Velocity
```python
velocity = telemetry['velocity'] # {x, y, z} in m/s
```
### Camera
The drone has a downward-facing camera providing 320x240 JPEG images.
```python
import base64
from PIL import Image
import io
camera = telemetry['camera']
image_b64 = camera.get('image')
if image_b64:
image_bytes = base64.b64decode(image_b64)
image = Image.open(io.BytesIO(image_bytes))
# Process image for custom vision algorithms
```
### Landing Pad (Vision)
**Important: May be None if pad not visible!**
```python
landing_pad = telemetry['landing_pad']
if landing_pad is not None:
relative_x = landing_pad['relative_x'] # body frame
relative_y = landing_pad['relative_y'] # body frame
distance = landing_pad['distance'] # vertical
confidence = landing_pad['confidence'] # 0-1
# Landing Pad (may be None!)
landing_pad = telemetry.get('landing_pad')
if landing_pad:
relative_x = landing_pad['relative_x']
relative_y = landing_pad['relative_y']
```
## Control Output
@@ -112,7 +72,7 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry):
thrust = 0.5 * (0 - altitude) - 0.3 * vertical_vel
# Horizontal control
if landing_pad is not None:
if landing_pad:
pitch = 0.3 * landing_pad['relative_x'] - 0.2 * vel_x
roll = 0.3 * landing_pad['relative_y'] - 0.2 * vel_y
else:
@@ -124,84 +84,43 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry):
## Using the Camera
You can implement custom vision processing on the camera image:
```python
import cv2
import numpy as np
import base64
def process_camera(telemetry):
camera = telemetry.get('camera', {})
image_b64 = camera.get('image')
camera = telemetry.get('camera', {})
image_b64 = camera.get('image')
if not image_b64:
return None
# Decode JPEG
if image_b64:
image_bytes = base64.b64decode(image_b64)
nparr = np.frombuffer(image_bytes, np.uint8)
image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
# Example: detect green landing pad
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
green_mask = cv2.inRange(hsv, (35, 50, 50), (85, 255, 255))
# Find contours
contours, _ = cv2.findContours(green_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if contours:
largest = max(contours, key=cv2.contourArea)
M = cv2.moments(largest)
if M['m00'] > 0:
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
# cx, cy is center of detected pad in image coordinates
return (cx, cy)
return None
# Process image...
```
## Strategies
### When Pad Not Visible
- Maintain altitude and stabilize
- Search by ascending or spiraling
- Dead reckoning from last known position
### State Machine
1. Search → find pad
2. Approach → move above pad
3. Align → center over pad
4. Descend → controlled descent
5. Land → touch down
## Testing
```bash
# Easy - stationary rover
# Easy - stationary
python standalone_simulation.py --pattern stationary
# Medium - slow circular movement
python standalone_simulation.py --pattern circular --speed 0.2
# Medium - circular
python standalone_simulation.py --pattern circular --speed 0.3
# Hard - faster random movement
python standalone_simulation.py --pattern random --speed 0.3
# With ROS 2 (Gazebo)
ros2 launch gazebo/launch/drone_landing.launch.py # Terminal 1
python run_gazebo.py --pattern circular # Terminal 2
# Hard - random
python standalone_simulation.py --pattern random --speed 0.5
```
## Configuration
Edit `config.py` to tune controller gains:
Edit `config.py`:
```python
CONTROLLER = {
"Kp_z": 0.5, # Altitude proportional gain
"Kd_z": 0.3, # Altitude derivative gain
"Kp_xy": 0.3, # Horizontal proportional gain
"Kd_xy": 0.2, # Horizontal derivative gain
"Kp_z": 0.5, # Altitude proportional
"Kd_z": 0.3, # Altitude derivative
"Kp_xy": 0.3, # Horizontal proportional
"Kd_xy": 0.2, # Horizontal derivative
}
```

167
docs/gazebo.md
View File

@@ -1,159 +1,72 @@
# Gazebo Simulation Guide
Running the GPS-denied drone simulation with Gazebo Ignition Fortress on Linux/WSL2.
## Quick Start (2 Terminals)
## Quick Start (Two Terminals)
**Terminal 1 - Launch Gazebo + Bridge:**
**Terminal 1 - Gazebo:**
```bash
source activate.sh
ros2 launch gazebo/launch/drone_landing.launch.py
```
**Terminal 2 - Run Controllers:**
**Terminal 2 - Controllers:**
```bash
source activate.sh
python run_gazebo.py --pattern circular --speed 0.3
python run_gazebo.py --pattern circular
```
Both the drone AND rover will move!
## Command Options
```bash
python run_gazebo.py --help
python run_gazebo.py --pattern circular --speed 0.5
Options:
--pattern stationary, linear, circular, square, random
--pattern, -p stationary, linear, circular, square, random
--speed, -s Rover speed in m/s (default: 0.5)
--amplitude, -a Movement amplitude (default: 2.0)
--amplitude, -a Movement radius (default: 2.0)
--no-rover Disable rover controller
```
## How It Works
1. **Gazebo** runs the physics simulation with:
- Drone with `VelocityControl` plugin (responds to `/drone/cmd_vel`)
- Rover with `VelocityControl` plugin (responds to `/rover/cmd_vel`)
2. **ros_gz_bridge** connects Gazebo topics to ROS 2
3. **run_gazebo.py** starts:
- `GazeboBridge` - converts ROS topics to telemetry format
- `DroneController` - your landing algorithm
- `RoverController` - moves the landing pad
## GPS-Denied Sensors
The `GazeboBridge` provides the same sensor interface as PyBullet:
| Sensor | Source |
|--------|--------|
| IMU | Gazebo odometry orientation |
| Altimeter | Gazebo Z position |
| Velocity | Gazebo twist |
| Camera | Gazebo camera sensor (if enabled) |
| Landing Pad | Computed from relative position |
## Topics
### ROS 2 Topics (your code uses these)
| Topic | Type | Direction |
|-------|------|-----------|
| `/cmd_vel` | `Twist` | Input (from DroneController) |
| `/drone/telemetry` | `String` | Output (to DroneController) |
| `/rover/telemetry` | `String` | Output (rover position) |
### Gazebo Topics (bridged automatically)
| Topic | Type | Description |
|-------|------|-------------|
| `/drone/cmd_vel` | `Twist` | Drone velocity commands |
| `/rover/cmd_vel` | `Twist` | Rover velocity commands |
| `/model/drone/odometry` | `Odometry` | Drone state |
| `/drone/imu` | `IMU` | IMU sensor data |
| `/clock` | `Clock` | Simulation time |
## Headless Mode (WSL2 / No GPU)
Run Gazebo without GUI:
## View Camera
```bash
# Server mode only
python camera_viewer.py --topic /drone/camera
```
## Sensors
| Sensor | Description |
|--------|-------------|
| IMU | Orientation, angular velocity |
| Altimeter | Altitude, vertical velocity |
| Velocity | Estimated velocity (x, y, z) |
| Camera | Downward-facing image |
| Landing Pad | Relative position |
## ROS 2 Topics
| Topic | Direction |
|-------|-----------|
| `/cmd_vel` | Your commands → Drone |
| `/drone/telemetry` | Sensors → You |
| `/drone/camera` | Camera → You |
| `/rover/telemetry` | Rover position |
## Headless Mode (WSL2)
```bash
# Server only (no GUI)
ign gazebo -s gazebo/worlds/drone_landing.sdf
```
Then run the bridge manually:
```bash
ros2 run ros_gz_bridge parameter_bridge \
/drone/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist \
/rover/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist \
/model/drone/odometry@nav_msgs/msg/Odometry[ignition.msgs.Odometry
```
## World File Details
The world file `gazebo/worlds/drone_landing.sdf` includes:
- **Drone** at (0, 0, 2) with:
- `VelocityControl` plugin for movement
- `OdometryPublisher` plugin for telemetry
- IMU sensor
- **Landing Pad (Rover)** at (0, 0, 0.15) with:
- `VelocityControl` plugin for movement
- Visual H marker
## Troubleshooting
### Drone falls immediately
**Drone falls:**
- Check `run_gazebo.py` is running
- Check topic: `ros2 topic echo /drone/cmd_vel`
The drone should hover with the controller running. If it falls:
1. Check that `run_gazebo.py` is running
2. Verify the bridge shows "Passing message from ROS"
3. Check `/drone/cmd_vel` topic: `ros2 topic echo /drone/cmd_vel`
**Rover doesn't move:**
- Check topic: `ros2 topic echo /rover/cmd_vel`
### Rover doesn't move
1. Check that `/rover/cmd_vel` is bridged
2. Verify RoverController is publishing: `ros2 topic echo /rover/cmd_vel`
### Model not found
Set the model path:
**Model not found:**
```bash
export GZ_SIM_RESOURCE_PATH=$PWD/gazebo/models:$GZ_SIM_RESOURCE_PATH
export IGN_GAZEBO_RESOURCE_PATH=$PWD/gazebo/models:$IGN_GAZEBO_RESOURCE_PATH
```
### "Cannot connect to display" (WSL2)
Use headless mode:
```bash
ign gazebo -s gazebo/worlds/drone_landing.sdf
```
Or ensure WSLg is working:
```bash
export DISPLAY=:0
```
### Plugin not found
For Ignition Fortress, plugins use `libignition-gazebo-*-system.so` naming.
Check available plugins:
```bash
ls /usr/lib/x86_64-linux-gnu/ign-gazebo-6/plugins/
```
## Launch File Options
```bash
ros2 launch gazebo/launch/drone_landing.launch.py use_sim_time:=true
```
| Argument | Default | Description |
|----------|---------|-------------|
| `use_sim_time` | `true` | Use Gazebo clock |

496
docs/installation.md
View File

@@ -1,503 +1,125 @@
# Installation Guide
Setup instructions for all supported platforms.
## Quick Install
```bash
# Ubuntu/Debian
./setup/install_ubuntu.sh
source activate.sh
# Test
python standalone_simulation.py
```
## Install Scripts
| Platform | Command |
|----------|---------|
| Ubuntu/Debian | `./setup/install_ubuntu.sh` |
| Ubuntu + ArduPilot | `./setup/install_ubuntu.sh --with-ardupilot` |
| ArduPilot SITL | `./setup/install_ardupilot.sh` |
| Arch Linux | `./setup/install_arch.sh` |
| macOS | `./setup/install_macos.sh` |
| Windows | `.\setup\install_windows.ps1` |
After installation:
```bash
source activate.sh # Linux/macOS
. .\activate.ps1 # Windows PowerShell
## Platform Support
python standalone_simulation.py
```
| Mode | Ubuntu | macOS | Windows |
|------|--------|-------|---------|
| Standalone | ✅ | ✅ | ✅ |
| Gazebo + ROS 2 | ✅ | ❌ | WSL2 |
| ArduPilot SITL | ✅ | ❌ | WSL2 |
---
## Platform Compatibility
| Feature | Ubuntu | Arch | macOS | Windows | WSL2 |
|---------|--------|------|-------|---------|------|
| **Standalone Simulation** | ✅ | ✅ | ✅ | ✅ | ✅ |
| **ROS 2** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ |
| **Gazebo** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ |
| **ArduPilot SITL** | ✅ | ⚠️ Manual | ❌ | ❌ | ✅ |
| **Full Mode** | ✅ | ⚠️ | ❌ | ❌ | ✅ |
| **GUI Support** | ✅ | ✅ | ✅ | ✅ | ✅ WSLg |
**Legend:**
- ✅ Fully supported
- ⚠️ Available but requires extra setup
- ❌ Not supported
**Recommendation for Windows users:** Use WSL2 for the full experience (ROS 2 + Gazebo).
---
## Ubuntu / Debian
**Tested on:** Ubuntu 22.04 (Jammy), Ubuntu 24.04 (Noble)
## Ubuntu/Debian
```bash
# Run installer
./setup/install_ubuntu.sh
# Activate environment
source activate.sh
# Run simulation
python standalone_simulation.py
```
**Installs:**
- ROS 2 (Humble or Jazzy based on Ubuntu version)
- Gazebo (ros-gz)
- Python packages: pybullet, numpy, pillow, pyinstaller, pymavlink
- ROS 2 Humble/Jazzy
- Gazebo
- Python packages (pybullet, numpy, opencv, pymavlink)
---
## ArduPilot SITL
For realistic flight controller simulation:
**With ArduPilot SITL (full flight controller):**
```bash
# Run installer with ArduPilot
./setup/install_ubuntu.sh --with-ardupilot
./setup/install_ardupilot.sh
source ~/.bashrc
```
# This will also install:
# - ArduPilot SITL (~15-20 min build)
# - ArduPilot Gazebo plugin
# - MAVProxy
**Installs:**
- ArduPilot SITL
- ardupilot_gz (Gazebo integration)
- MAVProxy
**Run:**
```bash
# Terminal 1
source ~/ardu_ws/install/setup.bash
ros2 launch ardupilot_gz_bringup iris_runway.launch.py
# Terminal 2
mavproxy.py --console --map --master=:14550
```
---
## Arch Linux
## Windows (WSL2)
**Tested on:** Arch Linux (rolling release)
```bash
# Run installer
./setup/install_arch.sh
# Activate environment
source activate.sh
# Run simulation
python standalone_simulation.py
1. Install WSL2:
```powershell
wsl --install -d Ubuntu-22.04
```
**Installs:**
- Python packages: pybullet, numpy, pillow, pyinstaller
- yay (AUR helper)
**Optional ROS 2 (from AUR):**
2. Open Ubuntu and run:
```bash
yay -S ros-humble-desktop
yay -S ros-humble-ros-gz
./setup/install_ubuntu.sh
source activate.sh
python standalone_simulation.py
```
---
## macOS
**Tested on:** macOS 12+ (Monterey, Ventura, Sonoma)
```bash
# Run installer
./setup/install_macos.sh
# Activate environment
source activate.sh
# Run simulation
python standalone_simulation.py
```
**Installs:**
- Homebrew (if not present)
- Python 3.11
- Python packages: pybullet, numpy, pillow, pyinstaller
**Note:** ROS 2 and Gazebo are not supported on macOS. Use standalone mode.
**Note:** ROS 2 and Gazebo not supported on macOS. Use standalone mode.
---
## Windows
**Tested on:** Windows 10, Windows 11
```powershell
# Open PowerShell as Administrator
Set-ExecutionPolicy RemoteSigned -Scope CurrentUser
# Run installer
.\setup\install_windows.ps1
# Activate environment
. .\activate.ps1
# Run simulation
python standalone_simulation.py
```
**Installs:**
- Chocolatey (package manager)
- Python 3.11
- Python packages: pybullet, numpy, pillow, pyinstaller
**Note:** ROS 2 and Gazebo are not supported natively on Windows. Use standalone mode or WSL2 (below).
---
## Windows with WSL2 (Full Linux Experience)
WSL2 lets you run full Linux on Windows with GUI support. This enables ROS 2 and Gazebo!
**Requirements:** Windows 10 (build 19041+) or Windows 11
### Step 1: Install WSL2
Open PowerShell as Administrator:
```powershell
# Install WSL2 with Ubuntu
wsl --install -d Ubuntu-22.04
# set the user name and password
# Then update the system
sudo apt update
sudo apt upgrade
# Restart computer if prompted
```
### Step 2: Enable GUI Support (WSLg)
Windows 11 and recent Windows 10 updates include WSLg (GUI support) automatically.
Verify by opening Ubuntu and running:
```bash
# Test GUI (should open a window)
sudo apt update
sudo apt install x11-apps -y
xclock
```
If xclock appears, GUI is working!
### Step 3: Install Simulation in WSL
Open Ubuntu from Start menu:
```bash
# Clone or copy your project
cd ~
git clone <your-repo-url> simulation
# OR copy from Windows:
# cp -r /mnt/c/Users/YourName/simulation ~/simulation
cd simulation
# Run Ubuntu installer
./setup/install_ubuntu.sh
# Activate
source activate.sh
# Run with GUI
python standalone_simulation.py
```
### WSL2 Tips
**Access Windows files:**
```bash
# Windows C: drive is at /mnt/c/
cd /mnt/c/Users/YourName/Documents
```
**Run from Windows Terminal:**
```powershell
wsl -d Ubuntu-22.04 -e bash -c "cd ~/simulation && source activate.sh && python standalone_simulation.py"
```
**GPU Acceleration (NVIDIA):**
If you have an NVIDIA GPU:
```bash
# Check if GPU is available
nvidia-smi
# PyBullet will use hardware rendering automatically
```
**Install Gazebo (optional):**
If you want to use Gazebo simulation:
```bash
# Install ros-gz bridge
sudo apt install ros-humble-ros-gz
# Install Gazebo Fortress (provides 'ign' command)
sudo apt install gz-fortress
# Verify - one of these should work:
gz sim --version # Newer Gazebo
ign gazebo --version # Fortress (ROS 2 Humble)
```
**Note:** ROS 2 Humble uses Gazebo Fortress, which uses `ign gazebo` command instead of `gz sim`. The launch file auto-detects which command is available.
**Gazebo GPU Issues in WSL2:**
If Gazebo crashes with GPU/OpenGL errors, try:
```bash
# Option 1: Run in server mode (no GUI)
ign gazebo -s gazebo/worlds/drone_landing.sdf
# Option 2: Fix permissions and restart WSL
sudo usermod -aG render $USER
chmod 700 /run/user/1000
# Then in PowerShell: wsl --shutdown
# Option 3: Force software rendering
export LIBGL_ALWAYS_SOFTWARE=1
ign gazebo gazebo/worlds/drone_landing.sdf
# Option 4: Just use PyBullet (more reliable on WSL2)
python standalone_simulation.py
```
**Troubleshooting WSL GUI:**
If GUI doesn't work:
```bash
# Update WSL
wsl --update
# Set WSL2 as default
wsl --set-default-version 2
# Reinstall Ubuntu
wsl --unregister Ubuntu-22.04
wsl --install -d Ubuntu-22.04
```
**Using VcXsrv (older Windows 10):**
If WSLg isn't available:
```powershell
# Install VcXsrv
choco install vcxsrv -y
```
Then in WSL:
```bash
# Add to ~/.bashrc
export DISPLAY=$(grep -m 1 nameserver /etc/resolv.conf | awk '{print $2}'):0
export LIBGL_ALWAYS_INDIRECT=1
# Start VcXsrv with "Disable access control" checked
# Then run simulation
python standalone_simulation.py
```
---
## Manual Installation
If the install scripts don't work, install manually:
### 1. Python 3.10+
```bash
# Ubuntu/Debian
sudo apt install python3 python3-pip python3-venv
# Arch
sudo pacman -S python python-pip python-virtualenv
# macOS
brew install python@3.11
# Windows
# Download from https://python.org
```
### 2. Create Virtual Environment
## Manual Install
```bash
# Create virtual environment
python3 -m venv venv
source venv/bin/activate # Linux/macOS
# OR
.\venv\Scripts\Activate.ps1 # Windows
```
source venv/bin/activate
### 3. Install Python Packages
```bash
# Install packages
pip install -r requirements.txt
```
Or manually:
```bash
pip install pybullet numpy pillow pyinstaller
```
### 4. Run Simulation
```bash
# Run
python standalone_simulation.py
```
---
## Troubleshooting
### PyBullet fails to install
Install build tools:
```bash
# Ubuntu/Debian
sudo apt install build-essential
# Arch
sudo pacman -S base-devel
# macOS
xcode-select --install
# Windows
# Install Visual Studio Build Tools
```
### "Cannot connect to X server"
PyBullet GUI requires a display:
```bash
# Use virtual display
sudo apt install xvfb
xvfb-run python standalone_simulation.py
# OR use X11 forwarding
ssh -X user@host
```
### Pillow fails to install
```bash
# Ubuntu/Debian
sudo apt install libjpeg-dev zlib1g-dev
# Arch
sudo pacman -S libjpeg-turbo zlib
# macOS
brew install libjpeg zlib
```
### Permission denied on Windows
Run PowerShell as Administrator:
```powershell
Set-ExecutionPolicy RemoteSigned -Scope CurrentUser
```
---
## Verification
After installation, verify packages:
```bash
python -c "import pybullet; print('PyBullet OK')"
python -c "import numpy; print('NumPy OK')"
python -c "from PIL import Image; print('Pillow OK')"
python -c "import cv2; print('OpenCV OK')"
python -c "from pymavlink import mavutil; print('pymavlink OK')"
```
All should print "OK".
---
## ArduPilot SITL Manual Setup
If you want to install ArduPilot SITL manually (without the install script):
### 1. Install ArduPilot
```bash
# Clone ArduPilot
git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git ~/ardupilot
cd ~/ardupilot
# Install prerequisites (Ubuntu)
Tools/environment_install/install-prereqs-ubuntu.sh -y
# Reload profile
. ~/.profile
# Build ArduCopter SITL
./waf configure --board sitl
./waf copter
```
### 2. Install ArduPilot Gazebo Plugin
```bash
# Clone plugin
git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo
cd ~/ardupilot_gazebo
# Build
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
```
### 3. Set Environment Variables
Add to `~/.bashrc`:
```bash
# ArduPilot
export ARDUPILOT_HOME=$HOME/ardupilot
export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest
# ArduPilot Gazebo Plugin
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
```
### 4. Test SITL
```bash
# Test ArduCopter SITL
cd ~/ardupilot
sim_vehicle.py -v ArduCopter --console --map
```
### 5. Run with Gazebo
```bash
# Terminal 1: Launch Gazebo
ros2 launch gazebo/launch/ardupilot_drone.launch.py
# Terminal 2: Start SITL
cd ~/ardupilot
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
# Terminal 3: Run controllers
cd ~/simulation
source activate.sh
python run_ardupilot.py --no-sitl --pattern circular
```
For more details, see [ArduPilot Guide](ardupilot.md).

165
docs/protocol.md
View File

@@ -1,8 +1,8 @@
# Communication Protocol (GPS-Denied)
# Communication Protocol
Message formats for GPS-denied drone operation with camera.
Message formats for drone operation.
## Drone Commands
## Commands
```json
{
@@ -13,25 +13,20 @@ Message formats for GPS-denied drone operation with camera.
}
```
| Field | Range | Description |
|-------|-------|-------------|
| `thrust` | ±1.0 | Vertical thrust (positive = up) |
| `pitch` | ±0.5 | Forward/backward tilt |
| `roll` | ±0.5 | Left/right tilt |
| `yaw` | ±0.5 | Rotation |
| Field | Range | Effect |
|-------|-------|--------|
| thrust | ±1.0 | Up/down |
| pitch | ±0.5 | Forward/back |
| roll | ±0.5 | Left/right |
| yaw | ±0.5 | Rotation |
---
## Drone Telemetry
Published on `/drone/telemetry`. **No GPS position available.**
## Telemetry
```json
{
"imu": {
"orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0},
"angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
"linear_acceleration": {"x": 0.0, "y": 0.0, "z": 9.81}
"angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}
},
"altimeter": {
"altitude": 5.0,
@@ -47,138 +42,30 @@ Published on `/drone/telemetry`. **No GPS position available.**
"camera": {
"width": 320,
"height": 240,
"fov": 60.0,
"image": "<base64 encoded JPEG>"
},
"landed": false,
"timestamp": 1.234
"image": "<base64 JPEG>"
}
}
```
---
## Sensors
## Sensor Details
| Sensor | Fields |
|--------|--------|
| IMU | orientation (roll, pitch, yaw), angular_velocity |
| Altimeter | altitude, vertical_velocity |
| Velocity | x, y, z (m/s) |
| Landing Pad | relative_x, relative_y, distance, confidence |
| Camera | Base64 JPEG image |
### IMU
Always available.
| Field | Unit | Description |
|-------|------|-------------|
| `orientation.roll/pitch/yaw` | radians | Euler angles |
| `angular_velocity.x/y/z` | rad/s | Rotation rates |
| `linear_acceleration.x/y/z` | m/s² | Acceleration |
### Altimeter
Always available.
| Field | Unit | Description |
|-------|------|-------------|
| `altitude` | meters | Height above ground |
| `vertical_velocity` | m/s | Vertical speed |
### Velocity
Estimated from optical flow.
| Field | Unit | Description |
|-------|------|-------------|
| `x` | m/s | Forward velocity |
| `y` | m/s | Lateral velocity |
| `z` | m/s | Vertical velocity |
### Landing Pad Detection
**May be null if pad not visible!**
| Field | Unit | Description |
|-------|------|-------------|
| `relative_x` | meters | Forward/back offset (body frame) |
| `relative_y` | meters | Left/right offset (body frame) |
| `distance` | meters | Vertical distance to pad |
| `confidence` | 0-1 | Detection confidence |
### Camera
Always available.
| Field | Description |
|-------|-------------|
| `width` | Image width in pixels |
| `height` | Image height in pixels |
| `fov` | Horizontal field of view in degrees |
| `image` | Base64 encoded JPEG (or null) |
---
## Using the Camera Image
The camera provides a base64-encoded JPEG image of what the drone sees looking down.
### Decoding the Image (Python)
```python
import base64
from PIL import Image
import io
def decode_camera_image(telemetry):
camera = telemetry.get('camera', {})
image_b64 = camera.get('image')
if image_b64 is None:
return None
# Decode base64 to bytes
image_bytes = base64.b64decode(image_b64)
# Load as PIL Image
image = Image.open(io.BytesIO(image_bytes))
return image
```
### Using with OpenCV
## Decoding Camera
```python
import base64
import cv2
import numpy as np
def decode_camera_image_cv2(telemetry):
camera = telemetry.get('camera', {})
image_b64 = camera.get('image')
if image_b64 is None:
return None
# Decode base64 to bytes
image_bytes = base64.b64decode(image_b64)
# Convert to numpy array
nparr = np.frombuffer(image_bytes, np.uint8)
# Decode JPEG
image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
return image
```
### Image Properties
- **Resolution**: 320 x 240 pixels
- **Format**: JPEG (quality 70)
- **FOV**: 60 degrees
- **Direction**: Downward-facing
- **Update Rate**: ~5 Hz (every 5th telemetry frame)
---
## Rover Telemetry
For internal use by RoverController.
```json
{
"position": {"x": 1.5, "y": 0.8, "z": 0.15},
"velocity": {"x": 0.3, "y": 0.4, "z": 0.0},
"pattern": "circular",
"timestamp": 1.234
}
image_b64 = telemetry['camera']['image']
image_bytes = base64.b64decode(image_b64)
nparr = np.frombuffer(image_bytes, np.uint8)
image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
```

155
docs/pybullet.md
View File

@@ -1,163 +1,68 @@
# PyBullet Simulation Guide
Running the GPS-denied drone simulation with PyBullet physics engine.
## Standalone Mode (1 Terminal)
## Standalone Mode (Single Terminal - Any Platform)
No ROS 2 required! Works on Windows, macOS, and Linux:
No ROS 2 required. Works on Windows, macOS, Linux:
```bash
source activate.sh # Linux/macOS
. .\activate.ps1 # Windows
python standalone_simulation.py --pattern circular --speed 0.3
source activate.sh
python standalone_simulation.py --pattern circular
```
### Options
```bash
python standalone_simulation.py --help
Options:
--pattern, -p stationary, linear, circular, square
--speed, -s Rover speed in m/s (default: 0.5)
--amplitude, -a Movement amplitude in meters (default: 2.0)
```
---
## ROS 2 Mode (Two Terminals)
For distributed or remote simulation with ROS 2:
## ROS 2 Mode (2 Terminals)
**Terminal 1 - Simulator:**
```bash
source activate.sh
python simulation_host.py
```
**Terminal 2 - Controllers:**
```bash
source activate.sh
python run_bridge.py --pattern circular --speed 0.3
python run_bridge.py --pattern circular
```
### How It Works
## Options
1. `simulation_host.py` runs PyBullet physics and listens on UDP port 5555
2. `run_bridge.py` starts:
- `ROS2SimulatorBridge` - connects ROS topics to UDP
- `DroneController` - your landing algorithm
- `RoverController` - moves the landing pad
The rover position is sent to the simulator, so both drone AND rover move!
### Remote Setup
Run simulator on one machine, controllers on another:
**Machine 1 (with display):**
```bash
python simulation_host.py # Listens on 0.0.0.0:5555
--pattern, -p stationary, linear, circular, square, random
--speed, -s Rover speed in m/s (default: 0.5)
--amplitude, -a Movement radius (default: 2.0)
```
**Machine 2 (headless):**
```bash
python run_bridge.py --host 192.168.1.100 --pattern circular
```
## Remote Setup
---
**Machine 1:** `python simulation_host.py`
**Machine 2:** `python run_bridge.py --host <IP>`
## Configuration
All parameters are configurable in `config.py`:
```python
DRONE = {
"mass": 1.0,
"start_position": (0.0, 0.0, 5.0),
"thrust_scale": 15.0,
...
}
ROVER = {
"start_position": (0.0, 0.0, 0.15),
"default_pattern": "circular",
"default_speed": 0.5,
...
}
CONTROLLER = {
"Kp_z": 0.5,
"Kd_z": 0.3,
...
}
```
---
## Simulation Parameters
| Parameter | Value |
|-----------|-------|
| Physics Rate | 240 Hz |
| Telemetry Rate | 24 Hz |
| Drone Mass | 1.0 kg (configurable) |
| Rover Mass | Static (kinematic) |
| UDP Port | 5555 (commands), 5556 (telemetry) |
## GPS-Denied Sensors
## Sensors
| Sensor | Description |
|--------|-------------|
| **IMU** | Orientation (roll, pitch, yaw), angular velocity |
| **Altimeter** | Altitude above ground, vertical velocity |
| **Velocity** | Estimated horizontal velocity (x, y, z) |
| **Camera** | 320x240 downward-facing JPEG image |
| **Landing Pad** | Vision-based relative position when in camera FOV |
| IMU | Orientation, angular velocity |
| Altimeter | Altitude, vertical velocity |
| Velocity | Estimated velocity (x, y, z) |
| Camera | 320x240 downward JPEG |
| Landing Pad | Relative position |
## Troubleshooting
## Configuration
### "Cannot connect to X server"
Edit `config.py`:
PyBullet GUI requires a display:
```bash
# Use virtual display
xvfb-run python standalone_simulation.py
# Or use X11 forwarding
ssh -X user@host
```
### Drone flies erratically
Reduce control gains in `config.py`:
```python
CONTROLLER = {
"Kp_z": 0.3,
"Kd_z": 0.2,
"Kp_xy": 0.2,
"Kd_xy": 0.1,
"Kp_z": 0.5,
"Kd_z": 0.3,
"Kp_xy": 0.3,
"Kd_xy": 0.2,
}
```
### Camera image not appearing
## Troubleshooting
Install Pillow:
**"Cannot connect to X server":**
```bash
pip install pillow numpy
xvfb-run python standalone_simulation.py
```
### Rover not moving (ROS 2 mode)
Ensure `run_bridge.py` is used (not `ros_bridge.py` directly).
The rover controller must be running to send position updates.
### WSL2 GUI issues
Set display scaling:
```bash
export GDK_DPI_SCALE=1.0
export QT_SCALE_FACTOR=1.0
python standalone_simulation.py
```
**Drone flies erratically:**
Reduce gains in `config.py`

109
docs/rover_controller.md
View File

@@ -4,97 +4,58 @@ The RoverController creates a moving landing pad target.
## Usage
The rover controller is automatically included when running `controllers.py`:
```bash
# Stationary rover (default)
python controllers.py
# Stationary (default)
python standalone_simulation.py --pattern stationary
# Moving rover
python controllers.py --pattern circular --speed 0.3
# Moving
python standalone_simulation.py --pattern circular --speed 0.3
```
### Options
## Options
| Option | Short | Default | Description |
|--------|-------|---------|-------------|
| `--pattern` | `-p` | stationary | Movement pattern |
| `--speed` | `-s` | 0.5 | Speed in m/s |
| `--amplitude` | `-a` | 2.0 | Amplitude in meters |
| Option | Default | Description |
|--------|---------|-------------|
| `--pattern, -p` | stationary | Movement pattern |
| `--speed, -s` | 0.5 | Speed in m/s |
| `--amplitude, -a` | 2.0 | Radius in meters |
## Movement Patterns
## Patterns
### Stationary
```bash
python controllers.py --pattern stationary
```
Rover stays at origin. Best for initial testing.
### Linear
```bash
python controllers.py --pattern linear --speed 0.3 --amplitude 2.0
```
Oscillates along X-axis.
### Circular
```bash
python controllers.py --pattern circular --speed 0.5 --amplitude 2.0
```
Follows circular path of radius `amplitude`.
### Random
```bash
python controllers.py --pattern random --speed 0.3 --amplitude 2.0
```
Moves to random positions. Changes target every 3 seconds.
### Square
```bash
python controllers.py --pattern square --speed 0.5 --amplitude 2.0
```
Square pattern with corners at `(±amplitude, ±amplitude)`.
| Pattern | Description |
|---------|-------------|
| stationary | Stays at origin |
| linear | Oscillates along X-axis |
| circular | Circular path |
| square | Square with sharp turns |
| random | Random positions |
## Difficulty Levels
| Level | Pattern | Speed | Description |
|-------|---------|-------|-------------|
| Beginner | stationary | 0.0 | Static target |
| Easy | linear | 0.2 | Predictable 1D |
| Medium | circular | 0.3 | Smooth 2D |
| Hard | random | 0.3 | Unpredictable |
| Expert | square | 0.5 | Sharp turns |
| Level | Pattern | Speed |
|-------|---------|-------|
| Beginner | stationary | 0.0 |
| Easy | linear | 0.2 |
| Medium | circular | 0.3 |
| Hard | random | 0.3 |
| Expert | square | 0.5 |
## Progressive Testing
Start easy and increase difficulty:
```bash
# Step 1: Static target
python controllers.py --pattern stationary
# 1. Static target
python standalone_simulation.py --pattern stationary
# Step 2: Slow linear motion
python controllers.py --pattern linear --speed 0.2
# 2. Slow circular
python standalone_simulation.py --pattern circular --speed 0.2
# Step 3: Slow circular motion
python controllers.py --pattern circular --speed 0.2
# 3. Faster circular
python standalone_simulation.py --pattern circular --speed 0.4
# Step 4: Faster circular
python controllers.py --pattern circular --speed 0.4
# Step 5: Random
python controllers.py --pattern random --speed 0.3
# 4. Random
python standalone_simulation.py --pattern random --speed 0.3
```
## Published Topics
## Note
| Topic | Type | Description |
|-------|------|-------------|
| `/rover/cmd_vel` | `Twist` | Velocity commands |
| `/rover/position` | `Point` | Current position |
| `/rover/telemetry` | `String` | Full state (JSON) |
## GPS-Denied Note
In GPS-denied mode, the drone cannot directly access rover position. Instead, it must detect the landing pad visually via `landing_pad` sensor data.
The `/rover/telemetry` topic is used internally by the RoverController but the DroneController should primarily rely on vision-based `landing_pad` detection in the drone telemetry.
The drone cannot access rover position directly (GPS-denied). It must detect the landing pad visually via the camera.

218
drone_controller.py
View File

@@ -1,16 +1,24 @@
#!/usr/bin/env python3
"""
DroneController - Template for GPS-denied landing logic.
Supports multiple telemetry sources:
1. Custom JSON telemetry (/drone/telemetry) - PyBullet/Gazebo bridge
2. ArduPilot ROS 2 DDS topics (/ap/*) - Official ArduPilot integration
Implement your algorithm in calculate_landing_maneuver().
Uses sensors: IMU, altimeter, camera, and landing pad detection.
"""
import json
import math
from typing import Dict, Any, Optional
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from geometry_msgs.msg import Twist, PoseStamped, TwistStamped
from sensor_msgs.msg import Imu, BatteryState, NavSatFix
from std_msgs.msg import String
# Load configuration
@@ -24,10 +32,38 @@ except ImportError:
LANDING = {"height_threshold": 0.1, "success_velocity": 0.1}
class DroneController(Node):
"""Drone controller for GPS-denied landing."""
def quaternion_to_euler(x: float, y: float, z: float, w: float) -> tuple:
"""Convert quaternion to Euler angles (roll, pitch, yaw)."""
# Roll (x-axis rotation)
sinr_cosp = 2.0 * (w * x + y * z)
cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(sinr_cosp, cosr_cosp)
def __init__(self):
# Pitch (y-axis rotation)
sinp = 2.0 * (w * y - z * x)
if abs(sinp) >= 1:
pitch = math.copysign(math.pi / 2, sinp)
else:
pitch = math.asin(sinp)
# Yaw (z-axis rotation)
siny_cosp = 2.0 * (w * z + x * y)
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
yaw = math.atan2(siny_cosp, cosy_cosp)
return roll, pitch, yaw
class DroneController(Node):
"""
Drone controller for GPS-denied landing.
Supports multiple telemetry sources:
- Legacy: /drone/telemetry (JSON String)
- ArduPilot DDS: /ap/pose/filtered, /ap/imu/filtered, etc.
"""
def __init__(self, use_ardupilot_topics: bool = True):
super().__init__('drone_controller')
# Load from config
@@ -42,37 +78,189 @@ class DroneController(Node):
self._landing_height = LANDING.get("height_threshold", 0.1)
self._landing_velocity = LANDING.get("success_velocity", 0.1)
# Mode selection
self._use_ardupilot = use_ardupilot_topics
self.get_logger().info('=' * 50)
self.get_logger().info('Drone Controller Starting (GPS-Denied)...')
if CONFIG_LOADED:
self.get_logger().info(' Configuration loaded from config.py')
self.get_logger().info(f' Mode: {"ArduPilot DDS" if use_ardupilot_topics else "Legacy JSON"}')
self.get_logger().info('=' * 50)
# State variables
self._latest_telemetry: Optional[Dict[str, Any]] = None
self._rover_telemetry: Optional[Dict[str, Any]] = None
self._telemetry_received = False
self._landing_complete = False
self._telemetry_sub = self.create_subscription(
String, '/drone/telemetry', self._telemetry_callback, 10
)
self.get_logger().info(' Subscribed to: /drone/telemetry')
# ArduPilot state (built from DDS topics)
self._ap_pose: Optional[PoseStamped] = None
self._ap_twist: Optional[TwistStamped] = None
self._ap_imu: Optional[Imu] = None
self._ap_battery: Optional[BatteryState] = None
# QoS for sensor topics
sensor_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1
)
if use_ardupilot_topics:
self._setup_ardupilot_subscriptions(sensor_qos)
else:
self._setup_legacy_subscriptions()
# Always subscribe to rover telemetry (for moving landing pad)
self._rover_telemetry_sub = self.create_subscription(
String, '/rover/telemetry', self._rover_telemetry_callback, 10
)
self.get_logger().info(' Subscribed to: /rover/telemetry')
# Command publisher
self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.get_logger().info(' Publishing to: /cmd_vel')
# Control loop
control_period = 1.0 / self._control_rate
self._control_timer = self.create_timer(control_period, self._control_loop)
self.get_logger().info(f' Control loop: {self._control_rate} Hz')
self.get_logger().info('Drone Controller Ready!')
self.get_logger().info('Sensors: IMU, Altimeter, Camera, Landing Pad Detection')
self.get_logger().info('Sensors: IMU, Altimeter, Velocity, Landing Pad Detection')
def _setup_ardupilot_subscriptions(self, qos: QoSProfile):
"""Set up subscriptions to official ArduPilot ROS 2 topics."""
# Pose (position + orientation)
self._ap_pose_sub = self.create_subscription(
PoseStamped, '/ap/pose/filtered', self._ap_pose_callback, qos
)
self.get_logger().info(' Subscribed to: /ap/pose/filtered')
# Twist (velocity)
self._ap_twist_sub = self.create_subscription(
TwistStamped, '/ap/twist/filtered', self._ap_twist_callback, qos
)
self.get_logger().info(' Subscribed to: /ap/twist/filtered')
# IMU
self._ap_imu_sub = self.create_subscription(
Imu, '/ap/imu/filtered', self._ap_imu_callback, qos
)
self.get_logger().info(' Subscribed to: /ap/imu/filtered')
# Battery
self._ap_battery_sub = self.create_subscription(
BatteryState, '/ap/battery', self._ap_battery_callback, qos
)
self.get_logger().info(' Subscribed to: /ap/battery')
def _setup_legacy_subscriptions(self):
"""Set up subscriptions to legacy JSON telemetry."""
self._telemetry_sub = self.create_subscription(
String, '/drone/telemetry', self._telemetry_callback, 10
)
self.get_logger().info(' Subscribed to: /drone/telemetry')
# ArduPilot topic callbacks
def _ap_pose_callback(self, msg: PoseStamped):
self._ap_pose = msg
if not self._telemetry_received:
self._telemetry_received = True
self._warmup_count = 0
self.get_logger().info('First ArduPilot pose received!')
self._build_telemetry_from_ardupilot()
def _ap_twist_callback(self, msg: TwistStamped):
self._ap_twist = msg
self._build_telemetry_from_ardupilot()
def _ap_imu_callback(self, msg: Imu):
self._ap_imu = msg
self._build_telemetry_from_ardupilot()
def _ap_battery_callback(self, msg: BatteryState):
self._ap_battery = msg
self._build_telemetry_from_ardupilot()
def _build_telemetry_from_ardupilot(self):
"""Build telemetry dict from ArduPilot topics."""
telemetry = {}
# Position and orientation from pose
if self._ap_pose:
pos = self._ap_pose.pose.position
ori = self._ap_pose.pose.orientation
roll, pitch, yaw = quaternion_to_euler(ori.x, ori.y, ori.z, ori.w)
telemetry['position'] = {
'x': pos.x,
'y': pos.y,
'z': pos.z
}
telemetry['altimeter'] = {
'altitude': pos.z,
'vertical_velocity': self._ap_twist.twist.linear.z if self._ap_twist else 0.0
}
telemetry['imu'] = {
'orientation': {'roll': roll, 'pitch': pitch, 'yaw': yaw},
'angular_velocity': {'x': 0, 'y': 0, 'z': 0}
}
# Velocity from twist
if self._ap_twist:
twist = self._ap_twist.twist
telemetry['velocity'] = {
'x': twist.linear.x,
'y': twist.linear.y,
'z': twist.linear.z
}
if 'altimeter' in telemetry:
telemetry['altimeter']['vertical_velocity'] = twist.linear.z
# Angular velocity from IMU
if self._ap_imu:
if 'imu' not in telemetry:
telemetry['imu'] = {}
telemetry['imu']['angular_velocity'] = {
'x': self._ap_imu.angular_velocity.x,
'y': self._ap_imu.angular_velocity.y,
'z': self._ap_imu.angular_velocity.z
}
# Battery
if self._ap_battery:
telemetry['battery'] = {
'voltage': self._ap_battery.voltage,
'remaining': self._ap_battery.percentage * 100
}
# Landing pad detection (placeholder - would need vision processing)
# For now, calculate from rover telemetry if available
if self._rover_telemetry and self._ap_pose:
rover_pos = self._rover_telemetry.get('position', {})
rx = rover_pos.get('x', 0)
ry = rover_pos.get('y', 0)
dx = self._ap_pose.pose.position.x
dy = self._ap_pose.pose.position.y
dz = self._ap_pose.pose.position.z
rel_x = rx - dx
rel_y = ry - dy
distance = math.sqrt(rel_x**2 + rel_y**2 + dz**2)
telemetry['landing_pad'] = {
'relative_x': rel_x,
'relative_y': rel_y,
'distance': distance,
'confidence': 1.0 if distance < 10.0 else 0.0
}
self._latest_telemetry = telemetry
# Legacy callbacks
def _telemetry_callback(self, msg: String) -> None:
try:
self._latest_telemetry = json.loads(msg.data)
@@ -211,11 +399,19 @@ class DroneController(Node):
def main(args=None):
rclpy.init(args=args)
import sys
# Check for --ardupilot flag
use_ardupilot = '--ardupilot' in sys.argv or '-a' in sys.argv
# Remove our custom args before passing to rclpy
filtered_args = [a for a in (args or sys.argv) if a not in ['--ardupilot', '-a']]
rclpy.init(args=filtered_args)
controller = None
try:
controller = DroneController()
controller = DroneController(use_ardupilot_topics=use_ardupilot)
rclpy.spin(controller)
except KeyboardInterrupt:
print('\nShutting down...')

130
gazebo/launch/ardupilot_drone.launch.py
View File

@@ -1,40 +1,52 @@
#!/usr/bin/env python3
"""
ArduPilot SITL + Gazebo Launch File
Works with Gazebo Harmonic/Garden and ArduPilot SITL.
ArduPilot ROS 2 Launch Helper
This launch file:
1. Starts Gazebo with ArduPilot-compatible drone model
2. Sets up ROS-Gazebo bridge for telemetry
3. Optionally starts ArduPilot SITL
This file provides a helper for launching the official ArduPilot ROS 2 simulation.
For the full ArduPilot + Gazebo experience, use the official ardupilot_gz packages.
NOTE: ArduPilot SITL integration requires the ardupilot_gazebo plugin.
Install from: https://github.com/ArduPilot/ardupilot_gazebo
RECOMMENDED: Use the official launch files:
ros2 launch ardupilot_gz_bringup iris_runway.launch.py
This local launch file is for custom Gazebo worlds or fallback testing.
"""
import os
import shutil
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction, LogInfo
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
"""Generate the launch description."""
"""
Generate launch description for ArduPilot simulation.
This launch file:
1. Checks for official ardupilot_gz packages
2. Falls back to local Gazebo world if packages not found
3. Starts ROS-Gazebo bridge for additional topics
"""
# Get paths
script_dir = os.path.dirname(os.path.abspath(__file__))
gazebo_dir = os.path.dirname(script_dir)
world_file = os.path.join(gazebo_dir, 'worlds', 'ardupilot_drone.sdf')
world_file = os.path.join(gazebo_dir, 'worlds', 'drone_landing.sdf')
# Check for ArduPilot Gazebo plugin
plugin_paths = [
os.path.expanduser("~/ardupilot_gazebo/build"),
"/opt/ardupilot_gazebo/lib",
os.environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", ""),
]
ardupilot_plugin_found = any(os.path.exists(p) for p in plugin_paths if p)
# Check if official ArduPilot packages are available
try:
import subprocess
result = subprocess.run(
['ros2', 'pkg', 'prefix', 'ardupilot_gz_bringup'],
capture_output=True, text=True
)
official_pkg_available = result.returncode == 0
except Exception:
official_pkg_available = False
# Use simulation time
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
# Determine Gazebo command
if shutil.which('gz'):
@@ -44,37 +56,35 @@ def generate_launch_description():
else:
gz_cmd = ['gz', 'sim', '-r', world_file]
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
start_sitl = LaunchConfiguration('start_sitl', default='false')
# Set plugin path for ArduPilot
env = os.environ.copy()
plugin_search_paths = ':'.join([p for p in plugin_paths if p and os.path.exists(p)])
if plugin_search_paths:
env['GZ_SIM_SYSTEM_PLUGIN_PATH'] = plugin_search_paths + ':' + env.get('GZ_SIM_SYSTEM_PLUGIN_PATH', '')
actions = [
DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation clock'
),
]
DeclareLaunchArgument(
'start_sitl',
default_value='false',
description='Start ArduPilot SITL automatically'
),
if official_pkg_available:
# Use official ArduPilot packages
actions.append(
LogInfo(msg=[
'\n',
'=' * 60,
'\n RECOMMENDED: Use official ArduPilot launch files:\n',
' ros2 launch ardupilot_gz_bringup iris_runway.launch.py\n',
'=' * 60,
'\n',
])
)
# Start Gazebo with ArduPilot world
# Start Gazebo with local world (for custom scenarios)
actions.extend([
ExecuteProcess(
cmd=gz_cmd,
output='screen',
additional_env=env
),
# ROS-Gazebo Bridge for telemetry and commands
# Delayed start to wait for Gazebo
# ROS-Gazebo Bridge
TimerAction(
period=2.0,
actions=[
@@ -83,14 +93,13 @@ def generate_launch_description():
executable='parameter_bridge',
name='gz_bridge',
arguments=[
# Rover velocity commands (ROS to Gazebo)
# Rover velocity commands
'/rover/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
# Odometry (from Gazebo to ROS) - fallback if MAVLink not used
# Drone velocity commands
'/drone/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
# Odometry
'/model/drone/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
# Camera (from Gazebo to ROS)
'/drone/camera@sensor_msgs/msg/Image[gz.msgs.Image',
# IMU (from Gazebo to ROS)
'/imu@sensor_msgs/msg/Imu[gz.msgs.IMU',
'/model/rover/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
# Clock
'/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
],
@@ -99,36 +108,31 @@ def generate_launch_description():
),
]
),
]
])
return LaunchDescription(actions)
if __name__ == '__main__':
print("=" * 60)
print(" ArduPilot SITL + Gazebo Launch File")
print(" ArduPilot ROS 2 Launch Helper")
print("=" * 60)
print()
print("This is a ROS 2 launch file for ArduPilot integration.")
print("RECOMMENDED: Use official ArduPilot ROS 2 packages:")
print()
print("Prerequisites:")
print(" 1. Gazebo (gz sim or ign gazebo)")
print(" 2. ros_gz_bridge package")
print(" 3. ArduPilot Gazebo plugin (optional, for SITL)")
print(" # Install ArduPilot ROS 2")
print(" ./setup/install_ardupilot.sh")
print()
print("Usage:")
print(" # Source workspace")
print(" source ~/ardu_ws/install/setup.bash")
print()
print(" # Launch simulation (SITL + Gazebo + RViz)")
print(" ros2 launch ardupilot_gz_bringup iris_runway.launch.py")
print()
print(" # Connect MAVProxy")
print(" mavproxy.py --console --map --master=:14550")
print()
print("-" * 60)
print("Alternative: Use this local launch for custom worlds:")
print(" ros2 launch gazebo/launch/ardupilot_drone.launch.py")
print()
print("Then in another terminal:")
print(" # Start SITL")
print(" sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON")
print()
print(" # Or use the integrated runner")
print(" python run_ardupilot.py --no-sitl")
print()
print("Manual Gazebo Start:")
if shutil.which('gz'):
print(" gz sim -r gazebo/worlds/ardupilot_drone.sdf")
else:
print(" ign gazebo gazebo/worlds/ardupilot_drone.sdf")

504
gazebo/worlds/ardupilot_drone.sdf
View File

@@ -1,504 +0,0 @@
<?xml version="1.0" ?>
<!--
ArduPilot SITL + Gazebo Drone Landing World
This world integrates with ArduPilot SITL via the ardupilot_gazebo plugin.
The drone receives motor commands from ArduPilot and sends sensor data back.
Prerequisites:
1. ArduPilot Gazebo Plugin: https://github.com/ArduPilot/ardupilot_gazebo
2. ArduPilot SITL with JSON backend
Usage:
1. Start Gazebo: gz sim -r gazebo/worlds/ardupilot_drone.sdf
2. Start SITL: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON
3. Run bridge: python run_ardupilot.py --no-sitl
-->
<sdf version="1.9">
<world name="ardupilot_landing_world">
<!-- Physics - match ArduPilot defaults -->
<physics name="1ms" type="ode">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<!-- Gazebo Plugins -->
<plugin filename="gz-sim-physics-system"
name="gz::sim::systems::Physics"/>
<plugin filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands"/>
<plugin filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster"/>
<plugin filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu"/>
<plugin filename="gz-sim-air-pressure-system"
name="gz::sim::systems::AirPressure"/>
<plugin filename="gz-sim-altimeter-system"
name="gz::sim::systems::Altimeter"/>
<plugin filename="gz-sim-navsat-system"
name="gz::sim::systems::NavSat"/>
<!-- Lighting -->
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<direction>-0.5 0.1 -0.9</direction>
</light>
<light type="directional" name="fill_light">
<cast_shadows>false</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.3 0.3 0.3 1</diffuse>
<specular>0.0 0.0 0.0 1</specular>
<direction>0.5 0.3 -0.5</direction>
</light>
<!-- Ground Plane -->
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.5 0.5 0.5 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
</material>
</visual>
</link>
</model>
<!-- Landing Pad (Rover) - Velocity controlled for moving target -->
<model name="landing_pad">
<pose>0 0 0.15 0 0 0</pose>
<link name="base_link">
<inertial>
<mass>10.0</mass>
<inertia>
<ixx>1.0</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>1.0</iyy><iyz>0</iyz>
<izz>1.0</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box><size>1.0 1.0 0.3</size></box>
</geometry>
<material>
<ambient>0.1 0.5 0.1 1</ambient>
<diffuse>0.2 0.7 0.2 1</diffuse>
</material>
</visual>
<collision name="collision">
<geometry>
<box><size>1.0 1.0 0.3</size></box>
</geometry>
</collision>
<!-- H marker -->
<visual name="h_center">
<pose>0 0 0.151 0 0 0</pose>
<geometry><box><size>0.6 0.1 0.001</size></box></geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
</material>
</visual>
<visual name="h_left">
<pose>-0.25 0 0.151 0 0 0</pose>
<geometry><box><size>0.1 0.5 0.001</size></box></geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
</material>
</visual>
<visual name="h_right">
<pose>0.25 0 0.151 0 0 0</pose>
<geometry><box><size>0.1 0.5 0.001</size></box></geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
</material>
</visual>
</link>
<!-- Velocity control for rover -->
<plugin filename="gz-sim-velocity-control-system"
name="gz::sim::systems::VelocityControl">
<topic>/rover/cmd_vel</topic>
<initial_linear>0 0 0</initial_linear>
</plugin>
</model>
<!--
ArduPilot Iris Quadcopter
This model uses the ArduPilot Gazebo plugin for flight control.
Motor commands come from ArduPilot SITL, and sensor data is sent back.
-->
<model name="iris_with_ardupilot">
<pose>0 0 0.194923 0 0 0</pose>
<link name="base_link">
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>1.5</mass>
<inertia>
<ixx>0.029125</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.029125</iyy>
<iyz>0</iyz>
<izz>0.055225</izz>
</inertia>
</inertial>
<collision name="base_link_collision">
<geometry>
<box><size>0.47 0.47 0.11</size></box>
</geometry>
</collision>
<!-- Main body -->
<visual name="base_link_visual">
<geometry>
<box><size>0.3 0.3 0.1</size></box>
</geometry>
<material>
<ambient>0.8 0.1 0.1 1</ambient>
<diffuse>0.9 0.2 0.2 1</diffuse>
</material>
</visual>
<!-- Arms -->
<visual name="arm_fl">
<pose>0.12 0.12 0 0 0 0.785</pose>
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
</visual>
<visual name="arm_fr">
<pose>0.12 -0.12 0 0 0 -0.785</pose>
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
</visual>
<visual name="arm_bl">
<pose>-0.12 0.12 0 0 0 -0.785</pose>
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
</visual>
<visual name="arm_br">
<pose>-0.12 -0.12 0 0 0 0.785</pose>
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
</visual>
<!-- GPS antenna indicator -->
<visual name="gps_antenna">
<pose>0 0 0.08 0 0 0</pose>
<geometry><cylinder><radius>0.02</radius><length>0.04</length></cylinder></geometry>
<material><diffuse>0.2 0.2 0.8 1</diffuse></material>
</visual>
<!-- IMU Sensor -->
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>400</update_rate>
<topic>imu</topic>
<imu>
<angular_velocity>
<x><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></x>
<y><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></y>
<z><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></z>
</angular_velocity>
<linear_acceleration>
<x><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></x>
<y><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></y>
<z><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></z>
</linear_acceleration>
</imu>
</sensor>
<!-- Air Pressure Sensor (Barometer) -->
<sensor name="air_pressure" type="air_pressure">
<always_on>true</always_on>
<update_rate>50</update_rate>
<topic>air_pressure</topic>
<air_pressure>
<reference_altitude>0</reference_altitude>
<noise type="gaussian">
<mean>0</mean>
<stddev>0.01</stddev>
</noise>
</air_pressure>
</sensor>
<!-- Downward Camera for Landing Pad Detection -->
<sensor name="camera" type="camera">
<pose>0 0 -0.05 0 1.5708 0</pose>
<always_on>true</always_on>
<update_rate>30</update_rate>
<topic>/drone/camera</topic>
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
</sensor>
</link>
<!-- Rotor 0: Front Right (CCW) -->
<link name="rotor_0">
<pose>0.13 -0.22 0.023 0 0 0</pose>
<inertial>
<mass>0.025</mass>
<inertia>
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>1.66704e-04</iyy><iyz>0</iyz>
<izz>1.66704e-04</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
</visual>
<collision name="collision">
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
</collision>
</link>
<joint name="rotor_0_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_0</child>
<axis>
<xyz>0 0 1</xyz>
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
</axis>
</joint>
<!-- Rotor 1: Back Left (CCW) -->
<link name="rotor_1">
<pose>-0.13 0.2 0.023 0 0 0</pose>
<inertial>
<mass>0.025</mass>
<inertia>
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>1.66704e-04</iyy><iyz>0</iyz>
<izz>1.66704e-04</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
</visual>
<collision name="collision">
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
</collision>
</link>
<joint name="rotor_1_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_1</child>
<axis>
<xyz>0 0 1</xyz>
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
</axis>
</joint>
<!-- Rotor 2: Front Left (CW) -->
<link name="rotor_2">
<pose>0.13 0.22 0.023 0 0 0</pose>
<inertial>
<mass>0.025</mass>
<inertia>
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>1.66704e-04</iyy><iyz>0</iyz>
<izz>1.66704e-04</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
</visual>
<collision name="collision">
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
</collision>
</link>
<joint name="rotor_2_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_2</child>
<axis>
<xyz>0 0 1</xyz>
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
</axis>
</joint>
<!-- Rotor 3: Back Right (CW) -->
<link name="rotor_3">
<pose>-0.13 -0.2 0.023 0 0 0</pose>
<inertial>
<mass>0.025</mass>
<inertia>
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>1.66704e-04</iyy><iyz>0</iyz>
<izz>1.66704e-04</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
</visual>
<collision name="collision">
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
</collision>
</link>
<joint name="rotor_3_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_3</child>
<axis>
<xyz>0 0 1</xyz>
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
</axis>
</joint>
<!--
ArduPilot Plugin Configuration
This plugin connects to ArduPilot SITL via the JSON interface.
It receives motor commands and sends back sensor data.
fdm_addr: Address of ArduPilot SITL (default 127.0.0.1)
fdm_port_in: Port to receive motor commands (default 9002)
listen_addr: Our address for sending sensor data (127.0.0.1)
listen_port_out: SITL port for receiving sensor data (9003)
If ardupilot_gazebo plugin is not available, this will be ignored.
-->
<plugin filename="ArduPilotPlugin"
name="gz::sim::systems::ArduPilotPlugin">
<!-- Connection settings -->
<fdm_addr>127.0.0.1</fdm_addr>
<fdm_port_in>9002</fdm_port_in>
<listen_addr>127.0.0.1</listen_addr>
<listen_port_out>9003</listen_port_out>
<!-- Model reference link -->
<modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
<gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
<!-- IMU configuration -->
<imuName>imu_sensor</imuName>
<!-- Motor configuration (CCW positive, CW negative) -->
<!-- Rotor 0: FR CCW -->
<control channel="0">
<jointName>rotor_0_joint</jointName>
<type>VELOCITY</type>
<offset>0</offset>
<multiplier>838</multiplier>
<p_gain>0.2</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
</control>
<!-- Rotor 1: BL CCW -->
<control channel="1">
<jointName>rotor_1_joint</jointName>
<type>VELOCITY</type>
<offset>0</offset>
<multiplier>838</multiplier>
<p_gain>0.2</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
</control>
<!-- Rotor 2: FL CW -->
<control channel="2">
<jointName>rotor_2_joint</jointName>
<type>VELOCITY</type>
<offset>0</offset>
<multiplier>-838</multiplier>
<p_gain>0.2</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
</control>
<!-- Rotor 3: BR CW -->
<control channel="3">
<jointName>rotor_3_joint</jointName>
<type>VELOCITY</type>
<offset>0</offset>
<multiplier>-838</multiplier>
<p_gain>0.2</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
</control>
</plugin>
<!-- Odometry publisher for compatibility -->
<plugin filename="gz-sim-odometry-publisher-system"
name="gz::sim::systems::OdometryPublisher">
<odom_frame>odom</odom_frame>
<robot_base_frame>base_link</robot_base_frame>
<odom_topic>/model/drone/odometry</odom_topic>
<odom_publish_frequency>50</odom_publish_frequency>
</plugin>
</model>
</world>
</sdf>

812
mavlink_bridge.py
View File

@@ -1,579 +1,423 @@
#!/usr/bin/env python3
"""
MAVLink Bridge for ArduPilot SITL Integration.
Connects Gazebo simulation to ArduPilot SITL via MAVProxy.
MAVLink Command Interface for ArduPilot.
This bridge:
- Connects to ArduPilot SITL via MAVLink (UDP)
- Receives telemetry from ArduPilot (position, attitude, battery, etc.)
- Sends commands to ArduPilot (velocity, position, etc.)
- Publishes ROS 2 telemetry for the drone controller
This module provides a Python interface for sending MAVLink commands to ArduPilot
when using the official ROS 2 DDS integration. Telemetry is received via native
ROS 2 topics (/ap/*), but commands are still sent via MAVLink.
Usage:
from mavlink_bridge import MAVLinkCommander
cmd = MAVLinkCommander()
cmd.connect()
cmd.arm()
cmd.takeoff(5.0)
cmd.land()
"""
import json
import math
import time
import threading
from typing import Optional, Dict, Any, Tuple
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import String
from typing import Optional, Callable
from enum import Enum
try:
from pymavlink import mavutil
from pymavlink.dialects.v20 import ardupilotmega as mavlink
MAVLINK_AVAILABLE = True
PYMAVLINK_AVAILABLE = True
except ImportError:
MAVLINK_AVAILABLE = False
print("WARNING: pymavlink not installed. Run: pip install pymavlink")
PYMAVLINK_AVAILABLE = False
print("Warning: pymavlink not installed. Install with: pip install pymavlink")
class MAVLinkBridge(Node):
class FlightMode(Enum):
"""ArduCopter flight modes."""
STABILIZE = 0
ACRO = 1
ALT_HOLD = 2
AUTO = 3
GUIDED = 4
LOITER = 5
RTL = 6
CIRCLE = 7
LAND = 9
DRIFT = 11
SPORT = 13
FLIP = 14
AUTOTUNE = 15
POSHOLD = 16
BRAKE = 17
THROW = 18
AVOID_ADSB = 19
GUIDED_NOGPS = 20
SMART_RTL = 21
FLOWHOLD = 22
FOLLOW = 23
ZIGZAG = 24
SYSTEMID = 25
AUTOROTATE = 26
class MAVLinkCommander:
"""
ROS 2 Node that bridges between ROS topics and ArduPilot SITL via MAVLink.
MAVLink command interface for ArduPilot.
Provides methods for arming, disarming, mode changes, and flight commands.
"""
# MAVLink connection defaults
DEFAULT_SITL_HOST = "127.0.0.1"
DEFAULT_SITL_PORT = 14550 # MAVProxy output port
DEFAULT_SYSTEM_ID = 1
DEFAULT_COMPONENT_ID = 191 # MAV_COMP_ID_MISSIONPLANNER
def __init__(self, connection_string: str = "udpin:127.0.0.1:14550"):
"""
Initialize MAVLink commander.
# Telemetry rate (Hz)
TELEMETRY_RATE = 50.0
Args:
connection_string: MAVLink connection string (default for MAVProxy output)
"""
if not PYMAVLINK_AVAILABLE:
raise ImportError("pymavlink is required: pip install pymavlink")
def __init__(
self,
sitl_host: str = None,
sitl_port: int = None,
system_id: int = None,
component_id: int = None
):
super().__init__('mavlink_bridge')
# Connection parameters (with parameter overrides)
self.declare_parameter('sitl_host', sitl_host or self.DEFAULT_SITL_HOST)
self.declare_parameter('sitl_port', sitl_port or self.DEFAULT_SITL_PORT)
self.declare_parameter('system_id', system_id or self.DEFAULT_SYSTEM_ID)
self.declare_parameter('component_id', component_id or self.DEFAULT_COMPONENT_ID)
self._sitl_host = self.get_parameter('sitl_host').value
self._sitl_port = self.get_parameter('sitl_port').value
self._system_id = self.get_parameter('system_id').value
self._component_id = self.get_parameter('component_id').value
self.get_logger().info('=' * 60)
self.get_logger().info('MAVLink Bridge Starting...')
self.get_logger().info(f' Connecting to SITL at {self._sitl_host}:{self._sitl_port}')
self.get_logger().info('=' * 60)
# MAVLink connection
self._mav_conn: Optional[mavutil.mavlink_connection] = None
self._connection_string = connection_string
self._mav: Optional[mavutil.mavlink_connection] = None
self._connected = False
self._armed = False
self._flight_mode = "UNKNOWN"
self._heartbeat_thread: Optional[threading.Thread] = None
self._running = False
# Telemetry state
self._position = [0.0, 0.0, 0.0] # x, y, z (NED frame, converted to ENU)
self._velocity = [0.0, 0.0, 0.0] # vx, vy, vz
self._attitude = [0.0, 0.0, 0.0] # roll, pitch, yaw (radians)
self._angular_velocity = [0.0, 0.0, 0.0] # wx, wy, wz
self._battery_voltage = 0.0
self._battery_remaining = 100
self._gps_fix = 0
self._landed = False
self._home_position = None
self._last_heartbeat = 0.0
# Callbacks
self._on_heartbeat: Optional[Callable] = None
self._on_state_change: Optional[Callable] = None
# Rover position (for landing pad detection)
self._rover_position = [0.0, 0.0, 0.15]
def connect(self, timeout: float = 30.0) -> bool:
"""
Connect to ArduPilot via MAVLink.
# Thread safety
self._lock = threading.Lock()
Args:
timeout: Connection timeout in seconds
# ROS 2 Subscriptions
self._cmd_vel_sub = self.create_subscription(
Twist, '/cmd_vel', self._cmd_vel_callback, 10
)
self.get_logger().info(' Subscribed to: /cmd_vel')
self._rover_sub = self.create_subscription(
String, '/rover/telemetry', self._rover_callback, 10
)
self.get_logger().info(' Subscribed to: /rover/telemetry')
# ROS 2 Publishers
self._telemetry_pub = self.create_publisher(
String, '/drone/telemetry', 10
)
self.get_logger().info(' Publishing to: /drone/telemetry')
self._status_pub = self.create_publisher(
String, '/mavlink/status', 10
)
self.get_logger().info(' Publishing to: /mavlink/status')
# Initialize MAVLink connection
self._connect_mavlink()
# Start telemetry thread
self._running = True
self._mavlink_thread = threading.Thread(target=self._mavlink_loop, daemon=True)
self._mavlink_thread.start()
# Telemetry timer
telemetry_period = 1.0 / self.TELEMETRY_RATE
self._telemetry_timer = self.create_timer(telemetry_period, self._publish_telemetry)
self.get_logger().info('MAVLink Bridge Ready!')
def _connect_mavlink(self) -> bool:
"""Establish MAVLink connection to ArduPilot SITL."""
if not MAVLINK_AVAILABLE:
self.get_logger().error('pymavlink not available!')
return False
Returns:
True if connected successfully
"""
print(f"Connecting to {self._connection_string}...")
try:
connection_string = f'udpin:{self._sitl_host}:{self._sitl_port}'
self.get_logger().info(f'Connecting to: {connection_string}')
self._mav_conn = mavutil.mavlink_connection(
connection_string,
source_system=self._system_id,
source_component=self._component_id
)
self._mav = mavutil.mavlink_connection(self._connection_string)
# Wait for heartbeat
self.get_logger().info('Waiting for heartbeat...')
self._mav_conn.wait_heartbeat(timeout=30)
heartbeat = self._mav.wait_heartbeat(timeout=timeout)
if heartbeat is None:
print("Connection failed: No heartbeat received")
return False
self._connected = True
self._last_heartbeat = time.time()
self.get_logger().info(
f'Connected! System: {self._mav_conn.target_system}, '
f'Component: {self._mav_conn.target_component}'
self._running = True
# Start heartbeat thread
self._heartbeat_thread = threading.Thread(
target=self._heartbeat_loop, daemon=True
)
self._heartbeat_thread.start()
# Request data streams
self._request_data_streams()
print(f"Connected! System: {self._mav.target_system}, Component: {self._mav.target_component}")
return True
except Exception as e:
self.get_logger().error(f'MAVLink connection failed: {e}')
self._connected = False
print(f"Connection failed: {e}")
return False
def _request_data_streams(self):
"""Request required data streams from ArduPilot."""
if not self._mav_conn:
return
# Request all data streams at high rate
streams = [
(mavutil.mavlink.MAV_DATA_STREAM_ALL, 10),
(mavutil.mavlink.MAV_DATA_STREAM_RAW_SENSORS, 50),
(mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, 5),
(mavutil.mavlink.MAV_DATA_STREAM_POSITION, 50),
(mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, 50), # Attitude
(mavutil.mavlink.MAV_DATA_STREAM_EXTRA2, 10), # VFR_HUD
]
for stream_id, rate in streams:
self._mav_conn.mav.request_data_stream_send(
self._mav_conn.target_system,
self._mav_conn.target_component,
stream_id,
rate,
1 # Start
)
def _mavlink_loop(self):
"""Background thread for receiving MAVLink messages."""
reconnect_delay = 2.0
while self._running:
if not self._connected:
time.sleep(reconnect_delay)
self._connect_mavlink()
continue
def disconnect(self):
"""Disconnect from ArduPilot."""
self._running = False
if self._heartbeat_thread:
self._heartbeat_thread.join(timeout=2.0)
if self._mav:
self._mav.close()
self._connected = False
print("Disconnected")
def _heartbeat_loop(self):
"""Background thread for receiving heartbeats and updating state."""
while self._running and self._mav:
try:
# Non-blocking message receive
msg = self._mav_conn.recv_match(blocking=True, timeout=0.1)
if msg is None:
# Check for connection timeout
if time.time() - self._last_heartbeat > 5.0:
self.get_logger().warning('Heartbeat timeout, reconnecting...')
self._connected = False
continue
self._process_mavlink_message(msg)
except Exception as e:
self.get_logger().error(f'MAVLink receive error: {e}')
self._connected = False
def _process_mavlink_message(self, msg):
"""Process incoming MAVLink messages."""
msg_type = msg.get_type()
with self._lock:
if msg_type == 'HEARTBEAT':
self._last_heartbeat = time.time()
msg = self._mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1.0)
if msg:
self._armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
self._flight_mode = mavutil.mode_string_v10(msg)
elif msg_type == 'LOCAL_POSITION_NED':
# Convert NED to ENU (ROS convention)
self._position = [msg.y, msg.x, -msg.z] # NED to ENU
self._velocity = [msg.vy, msg.vx, -msg.vz]
elif msg_type == 'GLOBAL_POSITION_INT':
# Backup position from GPS
if self._home_position is None:
self._home_position = (msg.lat, msg.lon, msg.alt)
elif msg_type == 'ATTITUDE':
self._attitude = [msg.roll, msg.pitch, msg.yaw]
self._angular_velocity = [msg.rollspeed, msg.pitchspeed, msg.yawspeed]
elif msg_type == 'SYS_STATUS':
self._battery_voltage = msg.voltage_battery / 1000.0 # mV to V
self._battery_remaining = msg.battery_remaining
elif msg_type == 'GPS_RAW_INT':
self._gps_fix = msg.fix_type
elif msg_type == 'EXTENDED_SYS_STATE':
# Check landed state
self._landed = (msg.landed_state == mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND)
def _cmd_vel_callback(self, msg: Twist):
"""Handle velocity commands from drone controller."""
if not self._connected or not self._mav_conn:
return
try:
# Convert ROS Twist to MAVLink velocity command
# linear.x = forward (pitch), linear.y = left (roll), linear.z = up (thrust)
# angular.z = yaw rate
# Use SET_POSITION_TARGET_LOCAL_NED for velocity control
# Type mask: velocity only (ignore position and acceleration)
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
)
# Convert velocities (ENU to NED)
vx = msg.linear.x # Forward (body frame pitch command)
vy = msg.linear.y # Left (body frame roll command)
vz = -msg.linear.z # Down (negative because NED)
yaw_rate = msg.angular.z
self._mav_conn.mav.set_position_target_local_ned_send(
0, # time_boot_ms (not used)
self._mav_conn.target_system,
self._mav_conn.target_component,
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
type_mask,
0, 0, 0, # position (ignored)
vx, vy, vz, # velocity
0, 0, 0, # acceleration (ignored)
0, yaw_rate # yaw, yaw_rate
)
except Exception as e:
self.get_logger().warning(f'Failed to send velocity command: {e}')
def _rover_callback(self, msg: String):
"""Receive rover position for landing pad detection."""
try:
data = json.loads(msg.data)
pos = data.get('position', {})
with self._lock:
self._rover_position = [
pos.get('x', 0.0),
pos.get('y', 0.0),
pos.get('z', 0.15)
]
except json.JSONDecodeError:
if self._on_heartbeat:
self._on_heartbeat(self._armed, self._flight_mode)
except Exception:
pass
def _get_landing_pad_detection(self) -> Optional[Dict[str, float]]:
"""Simulate landing pad detection based on relative position."""
# Camera parameters
CAMERA_FOV = 60.0
CAMERA_RANGE = 10.0
@property
def connected(self) -> bool:
return self._connected
dx = self._rover_position[0] - self._position[0]
dy = self._rover_position[1] - self._position[1]
dz = self._rover_position[2] - self._position[2]
@property
def armed(self) -> bool:
return self._armed
horizontal_dist = math.sqrt(dx**2 + dy**2)
vertical_dist = -dz # Height above rover
if vertical_dist <= 0 or vertical_dist > CAMERA_RANGE:
return None
fov_rad = math.radians(CAMERA_FOV / 2)
max_horizontal = vertical_dist * math.tan(fov_rad)
if horizontal_dist > max_horizontal:
return None
# Transform to body frame using yaw
yaw = self._attitude[2]
cos_yaw = math.cos(-yaw)
sin_yaw = math.sin(-yaw)
relative_x = dx * cos_yaw - dy * sin_yaw
relative_y = dx * sin_yaw + dy * cos_yaw
# Calculate confidence
angle = math.atan2(horizontal_dist, vertical_dist)
confidence = max(0.0, 1.0 - (angle / fov_rad))
confidence *= max(0.0, 1.0 - (vertical_dist / CAMERA_RANGE))
return {
"relative_x": round(relative_x, 4),
"relative_y": round(relative_y, 4),
"distance": round(vertical_dist, 4),
"confidence": round(confidence, 4)
}
def _publish_telemetry(self):
"""Publish drone telemetry to ROS 2 topic."""
with self._lock:
# Landing pad detection
pad_detection = self._get_landing_pad_detection()
telemetry = {
"imu": {
"orientation": {
"roll": round(self._attitude[0], 4),
"pitch": round(self._attitude[1], 4),
"yaw": round(self._attitude[2], 4)
},
"angular_velocity": {
"x": round(self._angular_velocity[0], 4),
"y": round(self._angular_velocity[1], 4),
"z": round(self._angular_velocity[2], 4)
},
"linear_acceleration": {
"x": 0.0,
"y": 0.0,
"z": 9.81
}
},
"altimeter": {
"altitude": round(self._position[2], 4),
"vertical_velocity": round(self._velocity[2], 4)
},
"velocity": {
"x": round(self._velocity[0], 4),
"y": round(self._velocity[1], 4),
"z": round(self._velocity[2], 4)
},
"position": {
"x": round(self._position[0], 4),
"y": round(self._position[1], 4),
"z": round(self._position[2], 4)
},
"landing_pad": pad_detection,
"camera": {
"width": 320,
"height": 240,
"fov": 60.0,
"image": None # Would need camera integration
},
"battery": {
"voltage": round(self._battery_voltage, 2),
"remaining": self._battery_remaining
},
"landed": self._landed,
"armed": self._armed,
"flight_mode": self._flight_mode,
"connected": self._connected,
"timestamp": round(time.time(), 4)
}
telemetry_msg = String()
telemetry_msg.data = json.dumps(telemetry)
self._telemetry_pub.publish(telemetry_msg)
# Publish status
status_msg = String()
status_msg.data = json.dumps({
"connected": self._connected,
"armed": self._armed,
"mode": self._flight_mode,
"gps_fix": self._gps_fix
})
self._status_pub.publish(status_msg)
# =========================================================================
# ArduPilot Control Commands
# =========================================================================
@property
def flight_mode(self) -> str:
return self._flight_mode
def arm(self, force: bool = False) -> bool:
"""Arm the drone."""
if not self._connected or not self._mav_conn:
"""
Arm the vehicle.
Args:
force: Force arm (bypass preflight checks)
Returns:
True if arm command was sent
"""
if not self._connected:
print("Not connected")
return False
self.get_logger().info('Arming drone...')
self._mav_conn.arducopter_arm()
return self._wait_for_arm(True, timeout=5.0)
print("Arming...")
param = 21196.0 if force else 0.0 # Force arm magic number
self._mav.mav.command_long_send(
self._mav.target_system,
self._mav.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0, # Confirmation
1, # Arm
param, # Force
0, 0, 0, 0, 0
)
# Wait for confirmation
time.sleep(0.5)
return True
def disarm(self, force: bool = False) -> bool:
"""Disarm the drone."""
if not self._connected or not self._mav_conn:
"""
Disarm the vehicle.
Args:
force: Force disarm
Returns:
True if disarm command was sent
"""
if not self._connected:
print("Not connected")
return False
self.get_logger().info('Disarming drone...')
self._mav_conn.arducopter_disarm()
return self._wait_for_arm(False, timeout=5.0)
print("Disarming...")
def _wait_for_arm(self, expected: bool, timeout: float = 5.0) -> bool:
"""Wait for arm/disarm to complete."""
start = time.time()
while time.time() - start < timeout:
if self._armed == expected:
param = 21196.0 if force else 0.0
self._mav.mav.command_long_send(
self._mav.target_system,
self._mav.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0,
0, # Disarm
param,
0, 0, 0, 0, 0
)
time.sleep(0.5)
return True
time.sleep(0.1)
return False
def set_mode(self, mode: str) -> bool:
"""Set flight mode (e.g., 'GUIDED', 'LAND', 'LOITER')."""
if not self._connected or not self._mav_conn:
"""
Set flight mode.
Args:
mode: Mode name (GUIDED, LAND, LOITER, etc.)
Returns:
True if mode change command was sent
"""
if not self._connected:
print("Not connected")
return False
self.get_logger().info(f'Setting mode to: {mode}')
mode_upper = mode.upper()
mode_id = self._mav.mode_mapping().get(mode_upper)
mode_mapping = self._mav_conn.mode_mapping()
if mode.upper() not in mode_mapping:
self.get_logger().error(f'Unknown mode: {mode}')
if mode_id is None:
print(f"Unknown mode: {mode}")
return False
mode_id = mode_mapping[mode.upper()]
self._mav_conn.set_mode(mode_id)
print(f"Setting mode to {mode_upper}...")
# Wait for mode change
start = time.time()
while time.time() - start < 5.0:
if self._flight_mode.upper() == mode.upper():
self._mav.mav.set_mode_send(
self._mav.target_system,
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
mode_id
)
time.sleep(0.5)
return True
time.sleep(0.1)
return False
def takeoff(self, altitude: float = 5.0) -> bool:
"""Take off to specified altitude."""
if not self._connected or not self._mav_conn:
"""
Command takeoff to specified altitude.
Args:
altitude: Target altitude in meters
Returns:
True if takeoff command was sent
"""
if not self._connected:
print("Not connected")
return False
self.get_logger().info(f'Taking off to {altitude}m...')
print(f"Taking off to {altitude}m...")
# Set GUIDED mode
if not self.set_mode('GUIDED'):
self.get_logger().error('Failed to set GUIDED mode')
return False
# Arm if not armed
if not self._armed:
if not self.arm():
self.get_logger().error('Failed to arm')
return False
# Command takeoff
self._mav_conn.mav.command_long_send(
self._mav_conn.target_system,
self._mav_conn.target_component,
self._mav.mav.command_long_send(
self._mav.target_system,
self._mav.target_component,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
0, # confirmation
0, 0, 0, 0, # params 1-4 (unused)
0, 0, # lat, lon (use current)
altitude # altitude
0,
0, 0, 0, 0, 0, 0,
altitude
)
return True
def land(self) -> bool:
"""Command the drone to land."""
if not self._connected or not self._mav_conn:
"""
Command landing.
Returns:
True if land command was sent
"""
if not self._connected:
print("Not connected")
return False
self.get_logger().info('Landing...')
return self.set_mode('LAND')
print("Landing...")
def goto_position(self, x: float, y: float, z: float, yaw: float = 0.0):
"""Go to a local position (ENU frame)."""
if not self._connected or not self._mav_conn:
return
# Convert ENU to NED
type_mask = (
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_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_RATE_IGNORE
self._mav.mav.command_long_send(
self._mav.target_system,
self._mav.target_component,
mavutil.mavlink.MAV_CMD_NAV_LAND,
0,
0, 0, 0, 0, 0, 0, 0
)
self._mav_conn.mav.set_position_target_local_ned_send(
return True
def goto_position_ned(self, north: float, east: float, down: float) -> bool:
"""
Go to position in NED frame relative to home.
Args:
north: North position (m)
east: East position (m)
down: Down position (m, negative is up)
Returns:
True if command was sent
"""
if not self._connected:
print("Not connected")
return False
print(f"Going to NED: ({north}, {east}, {down})")
self._mav.mav.set_position_target_local_ned_send(
0, # time_boot_ms
self._mav_conn.target_system,
self._mav_conn.target_component,
self._mav.target_system,
self._mav.target_component,
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
type_mask,
y, x, -z, # ENU to NED position
0, 0, 0, # velocity (ignored)
0, 0, 0, # acceleration (ignored)
yaw, 0 # yaw, yaw_rate
0b0000111111111000, # Position only
north, east, down, # Position
0, 0, 0, # Velocity
0, 0, 0, # Acceleration
0, 0 # Yaw, yaw_rate
)
def destroy_node(self):
"""Clean shutdown."""
self._running = False
if self._mavlink_thread.is_alive():
self._mavlink_thread.join(timeout=2.0)
if self._mav_conn:
self._mav_conn.close()
super().destroy_node()
return True
def set_velocity_ned(self, vn: float, ve: float, vd: float, yaw_rate: float = 0.0) -> bool:
"""
Set velocity in NED frame.
Args:
vn: North velocity (m/s)
ve: East velocity (m/s)
vd: Down velocity (m/s, positive is down)
yaw_rate: Yaw rate (rad/s)
Returns:
True if command was sent
"""
if not self._connected:
return False
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_LOCAL_NED,
0b0000011111000111, # Velocity only
0, 0, 0, # Position (ignored)
vn, ve, vd, # Velocity
0, 0, 0, # Acceleration
0, yaw_rate
)
return True
def set_param(self, param_id: str, value: float) -> bool:
"""
Set a parameter value.
Args:
param_id: Parameter name
value: Parameter value
Returns:
True if command was sent
"""
if not self._connected:
print("Not connected")
return False
print(f"Setting {param_id} = {value}")
self._mav.mav.param_set_send(
self._mav.target_system,
self._mav.target_component,
param_id.encode('utf-8'),
value,
mavutil.mavlink.MAV_PARAM_TYPE_REAL32
)
return True
def main(args=None):
if not MAVLINK_AVAILABLE:
print("ERROR: pymavlink is required. Install with: pip install pymavlink")
def main():
"""Example usage of MAVLinkCommander."""
if not PYMAVLINK_AVAILABLE:
print("pymavlink not installed")
return
print("\n" + "=" * 60)
print(" MAVLink Bridge for ArduPilot SITL")
print("=" * 60 + "\n")
cmd = MAVLinkCommander()
rclpy.init(args=args)
bridge = None
if not cmd.connect(timeout=10.0):
print("Failed to connect")
return
try:
bridge = MAVLinkBridge()
rclpy.spin(bridge)
except KeyboardInterrupt:
print('\nShutting down...')
finally:
if bridge:
bridge.destroy_node()
if rclpy.ok():
rclpy.shutdown()
print(f"Armed: {cmd.armed}")
print(f"Mode: {cmd.flight_mode}")
# Example commands (uncomment to use):
# cmd.set_mode("GUIDED")
# cmd.arm()
# cmd.takeoff(5.0)
# time.sleep(10)
# cmd.land()
input("Press Enter to disconnect...")
cmd.disconnect()
if __name__ == '__main__':

13
requirements.txt
View File

@@ -1,5 +1,16 @@
# Drone Simulation Requirements
# Install with: pip install -r requirements.txt
# Core simulation
pybullet>=3.2.5
numpy>=1.20.0
pillow>=9.0.0
pyinstaller>=6.0.0
# Camera viewer
opencv-python>=4.5.0
# ArduPilot/MAVLink (optional - for ArduPilot mode)
pymavlink>=2.4.0
# Build executables (optional)
pyinstaller>=6.0.0

454
run_ardupilot.py
View File

@@ -1,16 +1,20 @@
#!/usr/bin/env python3
"""
Run ArduPilot SITL + Gazebo Simulation.
ArduPilot ROS 2 Launcher - Official DDS Integration.
This script orchestrates the full ArduPilot SITL + Gazebo simulation stack:
1. Launches ArduPilot SITL (sim_vehicle.py with Gazebo model)
2. Starts MAVProxy for GCS connectivity
3. Runs MAVLink bridge + controllers
This script provides a convenient way to launch the ArduPilot SITL simulation
using the official ardupilot_gz packages with ROS 2 DDS support.
Usage:
python run_ardupilot.py # Start everything
python run_ardupilot.py --pattern circular # With rover movement
python run_ardupilot.py --no-sitl # Just controllers (SITL already running)
python run_ardupilot.py # Launch Iris on runway
python run_ardupilot.py --world maze # Launch Iris in maze
python run_ardupilot.py --vehicle rover # Launch WildThumper rover
python run_ardupilot.py --mavproxy # Also start MAVProxy
Prerequisites:
- ArduPilot ROS 2 packages installed (./setup/install_ardupilot.sh)
- ROS 2 Humble/Jazzy sourced
- ~/ardu_ws workspace built and sourced
"""
import argparse
@@ -21,336 +25,208 @@ import sys
import time
from pathlib import Path
import rclpy
from rclpy.executors import MultiThreadedExecutor
# Check for pymavlink
try:
from pymavlink import mavutil
MAVLINK_AVAILABLE = True
except ImportError:
MAVLINK_AVAILABLE = False
from mavlink_bridge import MAVLinkBridge
from drone_controller import DroneController
from rover_controller import RoverController, MovementPattern
def check_ros2():
"""Check if ROS 2 is available."""
try:
subprocess.run(['ros2', '--help'], capture_output=True, check=True)
return True
except (subprocess.CalledProcessError, FileNotFoundError):
return False
def find_sim_vehicle() -> str:
"""Find sim_vehicle.py in common ArduPilot locations."""
locations = [
os.path.expanduser("~/ardupilot/Tools/autotest/sim_vehicle.py"),
"/opt/ardupilot/Tools/autotest/sim_vehicle.py",
os.environ.get("ARDUPILOT_HOME", "") + "/Tools/autotest/sim_vehicle.py",
def check_ardupilot_packages():
"""Check if ArduPilot ROS 2 packages are installed."""
try:
result = subprocess.run(
['ros2', 'pkg', 'list'],
capture_output=True,
text=True,
check=True
)
packages = result.stdout
return 'ardupilot_gz_bringup' in packages or 'ardupilot_sitl' in packages
except (subprocess.CalledProcessError, FileNotFoundError):
return False
def source_workspace():
"""Source the ArduPilot workspace."""
ardu_ws = os.path.expanduser("~/ardu_ws")
setup_bash = os.path.join(ardu_ws, "install", "setup.bash")
if os.path.exists(setup_bash):
# Update environment by sourcing the workspace
# This is done by running commands in a sourced shell
return True
return False
def get_launch_command(world: str, vehicle: str) -> list:
"""Get the appropriate launch command."""
launch_files = {
# Copter configurations
'runway': ('ardupilot_gz_bringup', 'iris_runway.launch.py'),
'maze': ('ardupilot_gz_bringup', 'iris_maze.launch.py'),
'iris': ('ardupilot_gz_bringup', 'iris_runway.launch.py'),
# Rover configurations
'rover': ('ardupilot_gz_bringup', 'wildthumper_playpen.launch.py'),
'wildthumper': ('ardupilot_gz_bringup', 'wildthumper_playpen.launch.py'),
# SITL only (no Gazebo)
'sitl': ('ardupilot_sitl', 'sitl_dds_udp.launch.py'),
}
if vehicle == 'rover':
key = 'rover'
else:
key = world if world in launch_files else 'runway'
package, launch_file = launch_files.get(key, launch_files['runway'])
cmd = ['ros2', 'launch', package, launch_file]
# Add SITL-specific parameters if using sitl_dds_udp
if launch_file == 'sitl_dds_udp.launch.py':
cmd.extend([
'transport:=udp4',
'synthetic_clock:=True',
'model:=quad' if vehicle == 'copter' else 'rover',
'speedup:=1',
])
return cmd
def launch_mavproxy(master_port: int = 14550):
"""Launch MAVProxy in a new terminal."""
mavproxy_cmd = f"mavproxy.py --console --map --master=:{master_port}"
# Try different terminal emulators
terminals = [
['gnome-terminal', '--', 'bash', '-c', mavproxy_cmd],
['xterm', '-e', mavproxy_cmd],
['konsole', '-e', mavproxy_cmd],
]
for loc in locations:
if os.path.exists(loc):
return loc
for term_cmd in terminals:
try:
subprocess.Popen(term_cmd)
return True
except FileNotFoundError:
continue
# Try finding it in PATH
import shutil
if shutil.which("sim_vehicle.py"):
return "sim_vehicle.py"
return None
print(f"[WARN] Could not open terminal for MAVProxy")
print(f"[INFO] Run manually: {mavproxy_cmd}")
return False
def parse_args():
parser = argparse.ArgumentParser(
description='Run ArduPilot SITL + Gazebo + MAVLink Bridge',
description='Launch ArduPilot SITL with ROS 2 and Gazebo',
formatter_class=argparse.RawDescriptionHelpFormatter,
epilog="""
Examples:
python run_ardupilot.py # Full stack with Gazebo
python run_ardupilot.py --pattern circular # Moving rover
python run_ardupilot.py --no-sitl # SITL already running
python run_ardupilot.py --mavproxy-port 14551 # Custom port
python run_ardupilot.py # Iris on runway
python run_ardupilot.py --world maze # Iris in maze
python run_ardupilot.py --vehicle rover # WildThumper rover
python run_ardupilot.py --mavproxy # With MAVProxy
Available worlds: runway, maze, sitl (no Gazebo)
Available vehicles: copter, rover
"""
)
# SITL options
parser.add_argument(
'--no-sitl', action='store_true',
help='Skip launching SITL (assume already running)'
'--world', '-w', type=str, default='runway',
choices=['runway', 'maze', 'sitl'],
help='Simulation world (default: runway)'
)
parser.add_argument(
'--sitl-path', type=str,
help='Path to sim_vehicle.py'
'--vehicle', '-v', type=str, default='copter',
choices=['copter', 'rover'],
help='Vehicle type (default: copter)'
)
parser.add_argument(
'--vehicle', type=str, default='ArduCopter',
choices=['ArduCopter', 'ArduPlane', 'APMrover2'],
help='Vehicle type (default: ArduCopter)'
'--mavproxy', '-m', action='store_true',
help='Also launch MAVProxy in a new terminal'
)
parser.add_argument(
'--frame', type=str, default='gazebo-iris',
help='Vehicle frame (default: gazebo-iris)'
)
# MAVProxy settings
parser.add_argument(
'--mavproxy-port', type=int, default=14550,
help='MAVProxy output port (default: 14550)'
)
parser.add_argument(
'--sitl-port', type=int, default=5760,
help='SITL connection port (default: 5760)'
help='MAVProxy master port (default: 14550)'
)
# Rover options
parser.add_argument(
'--pattern', type=str, default='stationary',
choices=['stationary', 'linear', 'circular', 'random', 'square'],
help='Rover movement pattern (default: stationary)'
)
parser.add_argument(
'--speed', '-s', type=float, default=0.5,
help='Rover speed in m/s (default: 0.5)'
)
parser.add_argument(
'--amplitude', '-a', type=float, default=2.0,
help='Rover movement amplitude in meters (default: 2.0)'
)
parser.add_argument(
'--no-rover', action='store_true',
help='Disable rover controller'
)
# Other
parser.add_argument(
'--console', action='store_true',
help='Show MAVProxy console'
)
parser.add_argument(
'--map', action='store_true',
help='Show MAVProxy map'
)
args, _ = parser.parse_known_args()
return args
class SITLProcess:
"""Manages the ArduPilot SITL process."""
def __init__(
self,
sim_vehicle_path: str,
vehicle: str = 'ArduCopter',
frame: str = 'gazebo-iris',
sitl_port: int = 5760,
mavproxy_port: int = 14550,
console: bool = False,
show_map: bool = False
):
self.sim_vehicle_path = sim_vehicle_path
self.vehicle = vehicle
self.frame = frame
self.sitl_port = sitl_port
self.mavproxy_port = mavproxy_port
self.console = console
self.show_map = show_map
self.process = None
def start(self) -> bool:
"""Start SITL process."""
if not self.sim_vehicle_path:
print("ERROR: sim_vehicle.py not found!")
print("Please set --sitl-path or ARDUPILOT_HOME environment variable")
return False
cmd = [
sys.executable, self.sim_vehicle_path,
'-v', self.vehicle,
'-f', self.frame,
'--model', 'JSON',
'--sitl-instance-args', f'--sim-address=127.0.0.1',
'--out', f'udp:127.0.0.1:{self.mavproxy_port}',
]
if self.console:
cmd.append('--console')
if self.show_map:
cmd.append('--map')
print(f"Starting SITL: {' '.join(cmd)}")
try:
self.process = subprocess.Popen(
cmd,
stdout=subprocess.PIPE,
stderr=subprocess.STDOUT
)
return True
except Exception as e:
print(f"Failed to start SITL: {e}")
return False
def stop(self):
"""Stop SITL process."""
if self.process:
self.process.terminate()
self.process.wait(timeout=5.0)
self.process = None
def wait_for_sitl(host: str = '127.0.0.1', port: int = 14550, timeout: float = 60.0) -> bool:
"""Wait for SITL to be ready."""
print(f"Waiting for SITL on {host}:{port}...")
start = time.time()
while time.time() - start < timeout:
try:
conn = mavutil.mavlink_connection(f'udpin:{host}:{port}', timeout=1)
if conn.wait_heartbeat(timeout=1):
print("SITL is ready!")
conn.close()
return True
except Exception:
pass
time.sleep(1)
print("Timeout waiting for SITL")
return False
return parser.parse_args()
def main():
args = parse_args()
if not MAVLINK_AVAILABLE:
print("ERROR: pymavlink is required!")
print("Install with: pip install pymavlink")
print("=" * 60)
print(" ArduPilot SITL + Gazebo (Official ROS 2 DDS)")
print("=" * 60)
print()
# Check ROS 2
if not check_ros2():
print("[ERROR] ROS 2 not found!")
print("Please source ROS 2:")
print(" source /opt/ros/humble/setup.bash")
return 1
print("=" * 60)
print(" ArduPilot SITL + Gazebo Simulation")
print("=" * 60)
print(f" Vehicle: {args.vehicle}")
print(f" Frame: {args.frame}")
print(f" Rover Pattern: {args.pattern}")
print(f" MAVProxy Port: {args.mavproxy_port}")
print("=" * 60)
print("[OK] ROS 2 available")
# Check ArduPilot packages
if not check_ardupilot_packages():
print("[ERROR] ArduPilot ROS 2 packages not found!")
print()
sitl = None
# Start SITL if not skipped
if not args.no_sitl:
sim_vehicle = args.sitl_path or find_sim_vehicle()
if not sim_vehicle:
print("=" * 60)
print("WARNING: sim_vehicle.py not found!")
print("Please install ArduPilot ROS 2:")
print(" ./setup/install_ardupilot.sh")
print()
print("Please either:")
print("1. Install ArduPilot: https://ardupilot.org/dev/docs/building-setup-linux-ubuntu-apt.html")
print("2. Set ARDUPILOT_HOME environment variable")
print("3. Use --sitl-path to specify location")
print("4. Use --no-sitl if SITL is already running")
print("=" * 60)
print()
print("Continuing without starting SITL...")
else:
sitl = SITLProcess(
sim_vehicle,
vehicle=args.vehicle,
frame=args.frame,
sitl_port=args.sitl_port,
mavproxy_port=args.mavproxy_port,
console=args.console,
show_map=args.map
)
if not sitl.start():
print("Then source the workspace:")
print(" source ~/ardu_ws/install/setup.bash")
return 1
print("\nWaiting for SITL to initialize...")
time.sleep(5) # Give SITL time to start
print("[OK] ArduPilot ROS 2 packages found")
# Wait for SITL connection
if not wait_for_sitl(port=args.mavproxy_port, timeout=30):
print("Could not connect to SITL!")
if sitl:
sitl.stop()
return 1
# Initialize ROS
rclpy.init()
nodes = []
executor = MultiThreadedExecutor(num_threads=4)
try:
# Create MAVLink bridge
bridge = MAVLinkBridge(
sitl_port=args.mavproxy_port
)
nodes.append(bridge)
executor.add_node(bridge)
print("[OK] MAVLink Bridge started")
# Create drone controller
drone = DroneController()
nodes.append(drone)
executor.add_node(drone)
print("[OK] Drone Controller started")
# Create rover controller
if not args.no_rover:
pattern = MovementPattern[args.pattern.upper()]
rover = RoverController(
pattern=pattern,
speed=args.speed,
amplitude=args.amplitude
)
nodes.append(rover)
executor.add_node(rover)
print("[OK] Rover Controller started")
# Get launch command
launch_cmd = get_launch_command(args.world, args.vehicle)
print()
print("=" * 60)
print("System ready!")
print()
print("To arm and takeoff, use MAVProxy commands:")
print(" mode guided")
print(" arm throttle")
print(" takeoff 5")
print()
print("Or use the bridge.arm() and bridge.takeoff() methods")
print("=" * 60)
print(f"World: {args.world}")
print(f"Vehicle: {args.vehicle}")
print(f"Launch: {' '.join(launch_cmd)}")
print()
# Launch MAVProxy if requested
if args.mavproxy:
print("[INFO] Starting MAVProxy...")
# Delay to let SITL start first
time.sleep(2)
launch_mavproxy(args.mavproxy_port)
print("Starting simulation...")
print("Press Ctrl+C to stop.")
print()
print("-" * 60)
# Handle Ctrl+C
# Handle Ctrl+C gracefully
def signal_handler(sig, frame):
print("\nShutting down...")
executor.shutdown()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
executor.spin()
except Exception as e:
print(f"Error: {e}")
raise
finally:
print("Cleaning up...")
# Stop nodes
for node in nodes:
node.destroy_node()
# Stop ROS
if rclpy.ok():
rclpy.shutdown()
# Stop SITL
if sitl:
sitl.stop()
print("Shutdown complete.")
# Run the launch command
try:
subprocess.run(launch_cmd, check=True)
except subprocess.CalledProcessError as e:
print(f"[ERROR] Launch failed: {e}")
return 1
except KeyboardInterrupt:
print("\nShutdown complete.")
return 0

351
setup/install_ardupilot.sh Executable file
View File

@@ -0,0 +1,351 @@
#!/bin/bash
# =============================================================================
# ArduPilot ROS 2 + Gazebo Installation Script
# =============================================================================
# Installs the official ArduPilot ROS 2 packages with DDS and Gazebo support.
# This is the recommended way to run ArduPilot SITL with ROS 2.
#
# Prerequisites:
# - Ubuntu 22.04 (ROS 2 Humble)
# - ROS 2 Humble installed
#
# Usage: ./install_ardupilot.sh [--skip-sitl] [--skip-gazebo]
#
# This installs:
# 1. ArduPilot SITL with DDS support
# 2. ardupilot_ros - ArduPilot ROS 2 packages
# 3. ardupilot_gz - ArduPilot Gazebo integration
# 4. MAVProxy ground control station
# =============================================================================
set -e
echo "=============================================="
echo " ArduPilot ROS 2 + Gazebo Installation"
echo "=============================================="
echo ""
# Parse arguments
SKIP_SITL=false
SKIP_GAZEBO=false
for arg in "$@"; do
case $arg in
--skip-sitl)
SKIP_SITL=true
;;
--skip-gazebo)
SKIP_GAZEBO=true
;;
esac
done
# Get script directory
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_ROOT="$(dirname "$SCRIPT_DIR")"
# Directories
ARDUPILOT_WS="$HOME/ardu_ws"
ARDUPILOT_DIR="$HOME/ardupilot"
# Check for ROS 2
if [ ! -f "/opt/ros/humble/setup.bash" ] && [ ! -f "/opt/ros/jazzy/setup.bash" ]; then
echo "[ERROR] ROS 2 not found!"
echo "Please install ROS 2 first:"
echo " ./setup/install_ubuntu.sh"
exit 1
fi
# Source ROS 2
if [ -f "/opt/ros/humble/setup.bash" ]; then
source /opt/ros/humble/setup.bash
ROS_DISTRO="humble"
GZ_VERSION="harmonic"
elif [ -f "/opt/ros/jazzy/setup.bash" ]; then
source /opt/ros/jazzy/setup.bash
ROS_DISTRO="jazzy"
GZ_VERSION="harmonic"
fi
echo "[INFO] ROS 2 Distro: $ROS_DISTRO"
echo "[INFO] Gazebo Version: $GZ_VERSION"
echo "[INFO] Workspace: $ARDUPILOT_WS"
echo ""
# -----------------------------------------------------------------------------
# Step 1: Install System Dependencies
# -----------------------------------------------------------------------------
echo "[STEP 1/7] Installing system dependencies..."
sudo apt-get update
sudo apt-get install -y \
git \
cmake \
build-essential \
python3 \
python3-pip \
python3-dev \
python3-venv \
python3-vcstool \
python3-rosdep \
python3-colcon-common-extensions \
wget \
curl
# Install additional dependencies
sudo apt-get install -y \
default-jre \
libxml2-dev \
libxslt1-dev \
libtool \
automake \
autoconf \
libexpat1-dev \
ccache || true
echo "[INFO] System dependencies installed"
# -----------------------------------------------------------------------------
# Step 2: Install Gazebo Harmonic
# -----------------------------------------------------------------------------
if [ "$SKIP_GAZEBO" = false ]; then
echo ""
echo "[STEP 2/7] Installing Gazebo $GZ_VERSION..."
# Add Gazebo APT sources
sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
sudo apt-get update
# Install Gazebo
sudo apt-get install -y gz-$GZ_VERSION || {
echo "[WARN] Could not install gz-$GZ_VERSION, trying gz-garden..."
sudo apt-get install -y gz-garden || true
}
echo "[INFO] Gazebo installed"
else
echo ""
echo "[STEP 2/7] Skipping Gazebo installation (--skip-gazebo)"
fi
# -----------------------------------------------------------------------------
# Step 3: Create ArduPilot ROS 2 Workspace
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 3/7] Setting up ArduPilot ROS 2 workspace..."
mkdir -p "$ARDUPILOT_WS/src"
cd "$ARDUPILOT_WS"
# Import ArduPilot ROS 2 repositories using vcstool
echo "[INFO] Importing ArduPilot ROS 2 repositories..."
# Create ros2.repos file for ArduPilot packages
cat > /tmp/ardupilot_ros2.repos << 'EOF'
repositories:
ardupilot:
type: git
url: https://github.com/ArduPilot/ardupilot.git
version: master
ardupilot_gz:
type: git
url: https://github.com/ArduPilot/ardupilot_gz.git
version: main
EOF
vcs import --recursive src < /tmp/ardupilot_ros2.repos
# Also import Gazebo-related packages
if [ "$SKIP_GAZEBO" = false ]; then
echo "[INFO] Importing Gazebo packages..."
vcs import --input https://raw.githubusercontent.com/ArduPilot/ardupilot_gz/main/ros2_gz.repos --recursive src || true
fi
echo "[INFO] ArduPilot ROS 2 repositories imported"
# -----------------------------------------------------------------------------
# Step 4: Install ArduPilot SITL Prerequisites
# -----------------------------------------------------------------------------
if [ "$SKIP_SITL" = false ]; then
echo ""
echo "[STEP 4/7] Installing ArduPilot SITL prerequisites..."
cd "$ARDUPILOT_WS/src/ardupilot"
# Run ArduPilot prerequisites installer
Tools/environment_install/install-prereqs-ubuntu.sh -y
# Reload profile
. ~/.profile || true
echo "[INFO] ArduPilot SITL prerequisites installed"
else
echo ""
echo "[STEP 4/7] Skipping SITL prerequisites (--skip-sitl)"
fi
# -----------------------------------------------------------------------------
# Step 5: Configure rosdep for Gazebo
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 5/7] Configuring rosdep..."
# Initialize rosdep if needed
if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then
sudo rosdep init || true
fi
rosdep update
# Add Gazebo sources to rosdep (for non-default ROS 2 Humble + Gazebo Harmonic pairing)
if [ "$ROS_DISTRO" = "humble" ] && [ "$GZ_VERSION" = "harmonic" ]; then
sudo wget https://raw.githubusercontent.com/osrf/osrf-rosdep/master/gz/00-gazebo.list \
-O /etc/ros/rosdep/sources.list.d/00-gazebo.list || true
rosdep update || true
fi
# Install ROS dependencies
cd "$ARDUPILOT_WS"
rosdep install --from-paths src --ignore-src -y || true
echo "[INFO] rosdep configured"
# -----------------------------------------------------------------------------
# Step 6: Build ArduPilot ROS 2 Packages
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 6/7] Building ArduPilot ROS 2 packages..."
cd "$ARDUPILOT_WS"
source /opt/ros/$ROS_DISTRO/setup.bash
# Set Gazebo version environment variable
export GZ_VERSION=$GZ_VERSION
# Build packages
if [ "$SKIP_GAZEBO" = false ]; then
echo "[INFO] Building with Gazebo support..."
colcon build --packages-up-to ardupilot_gz_bringup --symlink-install || {
echo "[WARN] Full build failed, trying core packages only..."
colcon build --packages-up-to ardupilot_sitl --symlink-install
}
else
echo "[INFO] Building SITL only..."
colcon build --packages-up-to ardupilot_sitl --symlink-install
fi
echo "[INFO] ArduPilot ROS 2 packages built"
# -----------------------------------------------------------------------------
# Step 7: Configure Environment Variables
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 7/7] Configuring environment variables..."
BASHRC_MARKER="# === ArduPilot ROS 2 Configuration ==="
if ! grep -q "$BASHRC_MARKER" ~/.bashrc; then
echo "" >> ~/.bashrc
echo "$BASHRC_MARKER" >> ~/.bashrc
echo "" >> ~/.bashrc
# Gazebo version
echo "# Gazebo Version" >> ~/.bashrc
echo "export GZ_VERSION=$GZ_VERSION" >> ~/.bashrc
echo "" >> ~/.bashrc
# ArduPilot workspace
echo "# ArduPilot ROS 2 Workspace" >> ~/.bashrc
echo "export ARDUPILOT_WS=$ARDUPILOT_WS" >> ~/.bashrc
echo "" >> ~/.bashrc
# ArduPilot SITL paths
echo "# ArduPilot SITL" >> ~/.bashrc
echo "export PATH=\$PATH:$ARDUPILOT_WS/src/ardupilot/Tools/autotest" >> ~/.bashrc
echo "" >> ~/.bashrc
# Source ROS 2 and workspace
echo "# Source ROS 2 and ArduPilot workspace" >> ~/.bashrc
echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
echo "if [ -f $ARDUPILOT_WS/install/setup.bash ]; then" >> ~/.bashrc
echo " source $ARDUPILOT_WS/install/setup.bash" >> ~/.bashrc
echo "fi" >> ~/.bashrc
echo "" >> ~/.bashrc
echo "[INFO] Environment variables added to ~/.bashrc"
else
echo "[INFO] Environment variables already configured in ~/.bashrc"
fi
# Install pymavlink in venv if it exists
if [ -d "$PROJECT_ROOT/venv" ]; then
echo "[INFO] Installing pymavlink in project venv..."
source "$PROJECT_ROOT/venv/bin/activate"
pip install pymavlink mavproxy || true
deactivate
fi
# Also install pymavlink and mavproxy globally
pip3 install --user pymavlink mavproxy || true
# -----------------------------------------------------------------------------
# Verification
# -----------------------------------------------------------------------------
echo ""
echo "=============================================="
echo " Verifying Installation"
echo "=============================================="
echo ""
# Source the workspace
source "$ARDUPILOT_WS/install/setup.bash" || true
# Check ROS 2 packages
ros2 pkg list | grep -q "ardupilot_sitl" && echo "[OK] ardupilot_sitl package" || echo "[WARN] ardupilot_sitl not found"
if [ "$SKIP_GAZEBO" = false ]; then
ros2 pkg list | grep -q "ardupilot_gz_bringup" && echo "[OK] ardupilot_gz_bringup package" || echo "[WARN] ardupilot_gz_bringup not found"
fi
# Check Gazebo
if command -v gz &> /dev/null; then
echo "[OK] Gazebo (gz command)"
elif command -v ign &> /dev/null; then
echo "[OK] Gazebo Fortress (ign command)"
else
echo "[WARN] Gazebo command not found"
fi
# Check MAVProxy
python3 -c "from pymavlink import mavutil" &> /dev/null && echo "[OK] pymavlink" || echo "[WARN] pymavlink not found"
# -----------------------------------------------------------------------------
# Complete
# -----------------------------------------------------------------------------
echo ""
echo "=============================================="
echo " ArduPilot ROS 2 Installation Complete!"
echo "=============================================="
echo ""
echo "IMPORTANT: Run the following to apply changes:"
echo " source ~/.bashrc"
echo ""
echo "Quick Start - Run SITL with ROS 2:"
echo " source ~/ardu_ws/install/setup.bash"
echo " ros2 launch ardupilot_sitl sitl_dds_udp.launch.py \\"
echo " transport:=udp4 \\"
echo " synthetic_clock:=True \\"
echo " model:=quad"
echo ""
echo "Quick Start - Run with Gazebo:"
echo " source ~/ardu_ws/install/setup.bash"
echo " ros2 launch ardupilot_gz_bringup iris_runway.launch.py"
echo ""
echo "MAVProxy (in another terminal):"
echo " mavproxy.py --console --map --master=:14550"
echo ""
echo "ROS 2 Topics:"
echo " ros2 topic list"
echo " ros2 topic echo /ap/geopose/filtered"
echo ""
echo "For more info, see: docs/ardupilot.md"
echo ""

62
setup/install_ubuntu.sh
View File

@@ -3,9 +3,11 @@
# Drone Simulation - Ubuntu/Debian Installation Script
# =============================================================================
# Installs ROS 2, Gazebo, PyBullet, and all required dependencies
# Includes optional ArduPilot SITL setup for realistic flight controller
# Use --with-ardupilot to also install ArduPilot SITL
#
# Usage: ./install_ubuntu.sh [--with-ardupilot]
# Usage:
# ./install_ubuntu.sh # Basic installation
# ./install_ubuntu.sh --with-ardupilot # Include ArduPilot SITL
# =============================================================================
set -e
@@ -242,62 +244,20 @@ python3 -c "from PIL import Image; print(' Pillow: OK')" || echo " Pillow: FAI
python3 -c "from pymavlink import mavutil; print(' pymavlink: OK')" || echo " pymavlink: FAILED"
# -----------------------------------------------------------------------------
# Step 9: Optional ArduPilot SITL Installation
# Step 9: ArduPilot SITL Installation (if requested)
# -----------------------------------------------------------------------------
if [ "$INSTALL_ARDUPILOT" = true ]; then
echo ""
echo "[STEP 9] Installing ArduPilot SITL..."
echo "[INFO] Calling dedicated ArduPilot install script..."
# Install ArduPilot dependencies
sudo apt-get install -y \
python3-dev \
python3-opencv \
python3-wxgtk4.0 \
python3-matplotlib \
python3-lxml \
libxml2-dev \
libxslt1-dev || true
# Clone ArduPilot if not exists
if [ ! -d "$HOME/ardupilot" ]; then
echo "[INFO] Cloning ArduPilot..."
git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git "$HOME/ardupilot"
cd "$HOME/ardupilot"
Tools/environment_install/install-prereqs-ubuntu.sh -y
cd "$PROJECT_ROOT"
# Call the dedicated ArduPilot install script
if [ -f "$SCRIPT_DIR/install_ardupilot.sh" ]; then
bash "$SCRIPT_DIR/install_ardupilot.sh"
else
echo "[INFO] ArduPilot already exists at $HOME/ardupilot"
echo "[ERROR] install_ardupilot.sh not found!"
echo "[INFO] Please run: ./setup/install_ardupilot.sh"
fi
# Clone ArduPilot Gazebo plugin if not exists
if [ ! -d "$HOME/ardupilot_gazebo" ]; then
echo "[INFO] Cloning ArduPilot Gazebo plugin..."
git clone https://github.com/ArduPilot/ardupilot_gazebo.git "$HOME/ardupilot_gazebo"
cd "$HOME/ardupilot_gazebo"
mkdir -p build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
cd "$PROJECT_ROOT"
else
echo "[INFO] ArduPilot Gazebo plugin already exists at $HOME/ardupilot_gazebo"
fi
# Add to bashrc
if ! grep -q "ARDUPILOT_HOME" ~/.bashrc; then
echo '' >> ~/.bashrc
echo '# ArduPilot SITL' >> ~/.bashrc
echo 'export ARDUPILOT_HOME=$HOME/ardupilot' >> ~/.bashrc
echo 'export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest' >> ~/.bashrc
fi
if ! grep -q "ardupilot_gazebo" ~/.bashrc; then
echo '' >> ~/.bashrc
echo '# ArduPilot Gazebo Plugin' >> ~/.bashrc
echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH' >> ~/.bashrc
echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH' >> ~/.bashrc
fi
echo "[INFO] ArduPilot SITL installed"
fi
echo ""