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) # Drone Landing Simulation (GPS-Denied)
A GPS-denied drone landing simulation using relative sensors (IMU, altimeter, camera, landing pad detection) with multiple backends: A GPS-denied drone landing simulation with multiple backends. Land a drone on a moving platform using only relative sensors (IMU, altimeter, camera).
- **PyBullet** - Lightweight physics simulation
- **Gazebo** - Full robotics simulator
- **ArduPilot SITL** - Realistic flight controller with MAVProxy
## Quick Start ## Quick Start
### Standalone Mode (Any Platform - No ROS 2 Required) ### Standalone Mode (No ROS 2 - Any Platform)
```bash ```bash
source activate.sh # Linux/macOS source activate.sh
. .\activate.ps1 # Windows
python standalone_simulation.py --pattern circular --speed 0.3 python standalone_simulation.py --pattern circular --speed 0.3
``` ```
### PyBullet + ROS 2 (Two Terminals) ### Gazebo + ROS 2 (Linux/WSL2)
**Terminal 1 - Simulator:** **Terminal 1:**
```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:**
```bash ```bash
ros2 launch gazebo/launch/drone_landing.launch.py ros2 launch gazebo/launch/drone_landing.launch.py
``` ```
**Terminal 2 - Run Controllers:** **Terminal 2:**
```bash ```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 ```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 ```bash
cd ~/ardupilot mavproxy.py --console --map --master=:14550
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
``` ```
## Installation ## 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` | | Ubuntu/Debian | `./setup/install_ubuntu.sh` |
| ArduPilot SITL | `./setup/install_ardupilot.sh` |
| Arch Linux | `./setup/install_arch.sh` | | Arch Linux | `./setup/install_arch.sh` |
| macOS | `./setup/install_macos.sh` | | macOS | `./setup/install_macos.sh` |
| Windows | `.\setup\install_windows.ps1` | | Windows | `.\setup\install_windows.ps1` |
## Platform Compatibility
| Feature | Ubuntu | Arch | macOS | Windows | WSL2 |
|---------|--------|------|-------|---------|------|
| Standalone | ✅ | ✅ | ✅ | ✅ | ✅ |
| PyBullet + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ |
| Gazebo + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ |
| ArduPilot + Gazebo | ✅ | ⚠️ | ❌ | ❌ | ✅ |
## Files ## Files
| File | Description | | File | Description |
|------|-------------| |------|-------------|
| `standalone_simulation.py` | All-in-one (no ROS 2 required) | | `standalone_simulation.py` | **All-in-one** (no ROS 2) |
| `simulation_host.py` | PyBullet simulator server | | `run_gazebo.py` | Gazebo controllers |
| `run_bridge.py` | PyBullet bridge + Controllers | | `run_ardupilot.py` | ArduPilot launcher |
| `run_gazebo.py` | Gazebo bridge + Controllers | | `camera_viewer.py` | Drone camera window |
| `run_ardupilot.py` | **ArduPilot SITL** + MAVLink bridge | | `drone_controller.py` | **Your landing algorithm** |
| `mavlink_bridge.py` | MAVLink ↔ ROS 2 bridge | | `config.py` | Configuration |
| `config.py` | **Configuration file** (edit to customize) |
| `drone_controller.py` | **Your landing algorithm** (edit this!) | ## Sensors Available
| `rover_controller.py` | Moving landing pad controller |
| 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 ## Configuration
Edit `config.py` to customize: Edit `config.py` to customize:
- Drone/rover starting positions - Drone/rover positions
- Physical properties (mass, size)
- Controller gains (Kp, Kd) - Controller gains (Kp, Kd)
- Landing detection thresholds - Camera settings
- Landing thresholds
## Command Line Options ## Command Line Options
```bash ```bash
# Standalone (no ROS 2) # Movement patterns
python standalone_simulation.py --pattern circular --speed 0.3 --pattern, -p stationary, linear, circular, square, random
--speed, -s Speed in m/s (default: 0.5)
# PyBullet + ROS 2 --amplitude, -a Amplitude in meters (default: 2.0)
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)
``` ```
## 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 ## Documentation
| Document | Description | | Document | Description |
|----------|-------------| |----------|-------------|
| [Installation](docs/installation.md) | Platform setup guides + WSL2 | | [Installation](docs/installation.md) | Full setup guide |
| [Architecture](docs/architecture.md) | System components diagram | | [Architecture](docs/architecture.md) | System overview |
| [Gazebo Guide](docs/gazebo.md) | Gazebo-specific instructions | | [ArduPilot](docs/ardupilot.md) | ArduPilot SITL guide |
| [PyBullet Guide](docs/pybullet.md) | PyBullet-specific instructions | | [Gazebo](docs/gazebo.md) | Gazebo guide |
| [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
```

103
build_exe.py
View File

@@ -7,6 +7,8 @@ Usage:
python build_exe.py # Build standalone_simulation python build_exe.py # Build standalone_simulation
python build_exe.py simulation_host # Build simulation_host python build_exe.py simulation_host # Build simulation_host
python build_exe.py standalone # Build standalone_simulation 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 python build_exe.py all # Build all
""" """
@@ -24,12 +26,25 @@ except ImportError as e:
print("Install with: pip install pyinstaller pybullet") print("Install with: pip install pyinstaller pybullet")
sys.exit(1) 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: def get_pybullet_data_path() -> str:
return pybullet_data.getDataPath() 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.""" """Build a single executable."""
script_dir = Path(__file__).parent script_dir = Path(__file__).parent
source_file = script_dir / source_name 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}', 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: if console:
build_args.append('--console') build_args.append('--console')
else: else:
@@ -92,7 +117,7 @@ def main():
'target', 'target',
nargs='?', nargs='?',
default='standalone', default='standalone',
choices=['standalone', 'simulation_host', 'all'], choices=['standalone', 'simulation_host', 'ardupilot', 'mavlink_bridge', 'camera_viewer', 'all'],
help='What to build (default: standalone)' help='What to build (default: standalone)'
) )
args = parser.parse_args() args = parser.parse_args()
@@ -102,9 +127,11 @@ def main():
print("=" * 60) print("=" * 60)
print(f"Platform: {platform.system()}") print(f"Platform: {platform.system()}")
print(f"PyBullet data: {get_pybullet_data_path()}") print(f"PyBullet data: {get_pybullet_data_path()}")
print(f"pymavlink: {'Available' if PYMAVLINK_AVAILABLE else 'Not installed'}")
success = True success = True
# Build standalone simulation
if args.target in ['standalone', 'all']: if args.target in ['standalone', 'all']:
success &= build_executable( success &= build_executable(
'standalone_simulation.py', 'standalone_simulation.py',
@@ -112,6 +139,7 @@ def main():
console=False console=False
) )
# Build simulation host
if args.target in ['simulation_host', 'all']: if args.target in ['simulation_host', 'all']:
success &= build_executable( success &= build_executable(
'simulation_host.py', 'simulation_host.py',
@@ -119,11 +147,81 @@ def main():
console=True 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()
print("=" * 60) print("=" * 60)
if success: if success:
print(" BUILD COMPLETE!") print(" BUILD COMPLETE!")
print(" Executables in: dist/") 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: else:
print(" BUILD FAILED!") print(" BUILD FAILED!")
sys.exit(1) sys.exit(1)
@@ -132,3 +230,4 @@ def main():
if __name__ == '__main__': if __name__ == '__main__':
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 = {
# Camera sensor settings (for simulation)
"width": 320, "width": 320,
"height": 240, "height": 240,
"fov": 60.0, # Field of view in degrees "fov": 60.0, # Field of view in degrees
"near_clip": 0.1, # Near clipping plane "near_clip": 0.1, # Near clipping plane
"far_clip": 100.0, # Far clipping plane "far_clip": 100.0, # Far clipping plane
"jpeg_quality": 70, # JPEG compression quality (1-100) "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 = { ARDUPILOT = {
# Vehicle type # Vehicle type
"vehicle": "ArduCopter", # ArduCopter, ArduPlane, APMrover2 "vehicle": "ArduCopter", # ArduCopter, ArduPlane, APMrover2
"frame": "gazebo-iris", # Gazebo model frame "frame": "iris", # Gazebo model (iris, wildthumper)
# SITL connection # ROS 2 workspace (set by install_ardupilot.sh)
"sitl_host": "127.0.0.1", # SITL host address "workspace": "~/ardu_ws",
"sitl_port": 5760, # SITL TCP port
# MAVProxy output (for MAVLink bridge) # Simulation worlds
"mavproxy_host": "127.0.0.1", "default_world": "runway", # runway, maze, sitl
"mavproxy_port": 14550, # MAVProxy UDP output
# Gazebo plugin connection (JSON interface) # MAVProxy connection (for GCS features)
"gazebo_fdm_port_in": 9002, # Port for motor commands "mavproxy_port": 14550, # MAVProxy UDP port
"gazebo_fdm_port_out": 9003, # Port for sensor data
# Arming requirements (for simulation, can be relaxed) # DDS configuration
"require_gps": False, # GPS required for arming "dds_enable": True, # Enable DDS for native ROS 2 topics
"require_ekf": True, # EKF required for arming "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 = {
# MAVLink system IDs # MAVLink system IDs
"system_id": 1, # Our system ID "system_id": 1, # Our system ID
"component_id": 191, # MAV_COMP_ID_MISSIONPLANNER "component_id": 191, # MAV_COMP_ID_MISSIONPLANNER
# Target system (usually the autopilot) # Target system (the autopilot)
"target_system": 1, "target_system": 1,
"target_component": 1, "target_component": 1,
# Connection timeout (seconds) # Connection
"connection_string": "udpin:127.0.0.1:14550",
# Timeouts (seconds)
"heartbeat_timeout": 5.0, "heartbeat_timeout": 5.0,
"connection_timeout": 30.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 # Architecture Overview
GPS-denied drone landing simulation with multiple operation modes.
## 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 │ │ standalone_simulation.py │
│ ┌──────────────────────────────────┐ │ │ ┌──────────────────────────────────┐ │
│ │ PyBullet Physics + Camera │ │ │ │ PyBullet Physics + Camera │ │
│ │ Built-in Landing Controller │ │ │ │ Built-in Controller │ │
│ │ Rover Movement Patterns │ │ │ │ Rover Movement │ │
│ │ Configuration from config.py │ │
│ └──────────────────────────────────┘ │ │ └──────────────────────────────────┘ │
└────────────────────────────────────────┘ └────────────────────────────────────────┘
``` ```
### 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 Terminal 1 Terminal 2
┌──────────────────┐ ┌──────────────────────────┐ ┌───────────────────┐ ┌───────────────────┐
│ simulation_host │◄─UDP───►│ run_bridge.py │ │ Gazebo + Bridge │◄──────►│ run_gazebo.py │
│ (PyBullet) │ │ ┌────────────────────┐ │ │ (Physics) │ ROS │ + Controllers │
│ Port 5555 │ │ │ ROS2SimulatorBridge│ │ └───────────────────┘ └───────────────────┘
│ │ │ │ DroneController │ │
│ │ │ │ RoverController │ │
└──────────────────┘ │ └────────────────────┘ │
└──────────────────────────┘
``` ```
Data flow: ### 3. ArduPilot SITL (2 Terminals)
- 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. Gazebo + ROS 2 Mode (Two Terminals, Linux/WSL2) **Terminal 1:**
```bash
``` ros2 launch ardupilot_gz_bringup iris_runway.launch.py
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 │ │ └──────────────────────────┘
│ └─────────────────────┘ │
└───────────────────────────┘
``` ```
Data flow: **Terminal 2:**
- RoverController publishes to `/rover/cmd_vel` → Gazebo moves rover ```bash
- Gazebo publishes odometry → GazeboBridge converts to telemetry mavproxy.py --console --map --master=:14550
- 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 │ │ └──────────────────┘ │
└──────────────┘ └─────────────────┘ └────────────────────────┘
``` ```
Data flow: ```
- ArduPilot SITL sends motor commands → Gazebo plugin controls drone ┌─────────────────────────────────────────────┐
- Gazebo plugin sends sensor data → ArduPilot SITL for state estimation │ Single Launch Command │
- MAVProxy outputs telemetry → MAVLinkBridge converts to ROS telemetry │ (Starts SITL + Gazebo + RViz) │
- DroneController receives telemetry, publishes velocity commands ├─────────────────────────────────────────────┤
- MAVLinkBridge sends MAVLink commands → ArduPilot SITL executes │ ArduPilot SITL ◄──► Gazebo ◄──► ROS 2 │
│ ▲ │
│ │ /ap/* topics │
│ ▼ │
│ MAVProxy (GCS) │
└─────────────────────────────────────────────┘
```
Key differences from simple Gazebo mode: ## Key Components
- 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)
## Components | Component | Description |
|-----------|-------------|
| File | Description | | `drone_controller.py` | Your landing algorithm |
|------|-------------| | `gazebo_bridge.py` | Gazebo ↔ ROS bridge |
| `config.py` | Central configuration (positions, physics, gains) | | `mavlink_bridge.py` | MAVLink commands |
| `standalone_simulation.py` | All-in-one simulation | | `camera_viewer.py` | Camera display |
| `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 |
## ROS 2 Topics ## ROS 2 Topics
| Topic | Type | Description | | Topic | Direction | Description |
|-------|------|-------------| |-------|-----------|-------------|
| `/cmd_vel` | `Twist` | Drone commands from DroneController | | `/drone/telemetry` | ← | Sensor data (JSON) |
| `/drone/cmd_vel` | `Twist` | Drone commands to Gazebo | | `/cmd_vel` | → | Velocity commands |
| `/drone/telemetry` | `String` | GPS-denied sensor data (JSON) | | `/drone/camera` | ← | Camera images |
| `/rover/cmd_vel` | `Twist` | Rover velocity to simulator | | `/rover/telemetry` | ← | Landing pad position |
| `/rover/telemetry` | `String` | Rover position (JSON) |
## Network Configuration ## ArduPilot Topics
All components default to `0.0.0.0` for network accessibility. | Topic | Type |
|-------|------|
### Remote Setup (PyBullet mode) | `/ap/pose/filtered` | Position |
| `/ap/twist/filtered` | Velocity |
**Machine 1 (with display):** | `/ap/imu/filtered` | IMU |
```bash | `/ap/battery` | Battery |
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 | ✅ | ⚠️ | ❌ | ❌ | ✅ |

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. Run the simulation with a realistic ArduPilot flight controller.
## 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
```
## Quick Start ## Quick Start
### Option 1: Integrated Launch (Recommended) **Terminal 1 - Simulation:**
This starts everything together:
```bash ```bash
# Terminal 1: Start Gazebo source ~/ardu_ws/install/setup.bash
ros2 launch gazebo/launch/ardupilot_drone.launch.py ros2 launch ardupilot_gz_bringup iris_runway.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
``` ```
### Option 2: Manual Setup **Terminal 2 - Control:**
```bash ```bash
# Terminal 1: Start Gazebo world mavproxy.py --console --map --master=:14550
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
``` ```
### Option 3: Full Automatic ## Installation
```bash ```bash
# Starts everything (requires SITL installed) ./setup/install_ardupilot.sh
python run_ardupilot.py --pattern circular --console --map source ~/.bashrc
``` ```
## Flight Operations This installs:
- ArduPilot SITL with DDS
- Gazebo with ardupilot_gz
- MAVProxy
### Using MAVProxy Commands ## MAVProxy Commands
Once connected, use MAVProxy to control the drone:
```bash ```bash
# Set GUIDED mode for algorithm control # Set mode
mode guided mode guided
# Arm motors # Arm
arm throttle arm throttle
# Take off to 5 meters # Takeoff
takeoff 5 takeoff 5
# Land # Land
mode 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 ```bash
from mavlink_bridge import MAVLinkBridge # List topics
ros2 topic list
# Create bridge # View position
bridge = MAVLinkBridge(sitl_port=14550) ros2 topic echo /ap/geopose/filtered
# Arm and takeoff # View battery
bridge.set_mode('GUIDED') ros2 topic echo /ap/battery
bridge.arm()
bridge.takeoff(altitude=5.0)
# Land
bridge.land()
``` ```
## Files | Topic | Type |
|-------|------|
| `/ap/pose/filtered` | PoseStamped |
| `/ap/twist/filtered` | TwistStamped |
| `/ap/imu/filtered` | Imu |
| `/ap/battery` | BatteryState |
| File | Description | ## Available Worlds
|------|-------------|
| `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 |
## 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 # Rover
ARDUPILOT = { ros2 launch ardupilot_gz_bringup wildthumper_playpen.launch.py
"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,
}
``` ```
## Telemetry Format ## Using the Launcher
The MAVLink bridge publishes telemetry in the same format as other modes: ```bash
python run_ardupilot.py --world runway
```json python run_ardupilot.py --world maze
{ python run_ardupilot.py --vehicle rover
"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
}
``` ```
## Troubleshooting ## Troubleshooting
### SITL Not Starting **No ROS 2 topics:**
```bash ```bash
# Check if SITL is installed # Check DDS is enabled
which sim_vehicle.py param set DDS_ENABLE 1
# Set ArduPilot path
export ARDUPILOT_HOME=~/ardupilot
export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest
``` ```
### Gazebo Plugin Not Found **Can't arm:**
```bash ```bash
# Check plugin path # Disable pre-arm checks (simulation only)
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
param set ARMING_CHECK 0 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 ## Quick Start
1. Edit `drone_controller.py` 1. Edit `drone_controller.py`
2. Find `calculate_landing_maneuver()` 2. Find `calculate_landing_maneuver()`
3. Implement your algorithm 3. Implement your algorithm
4. Test with any mode: 4. Test: `python standalone_simulation.py`
- `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)
## GPS-Denied Challenge ## Sensors Available
No GPS available. You must use: | Sensor | Description |
|--------|-------------|
| Sensor | Data | | IMU | Orientation, angular velocity |
|--------|------| | Altimeter | Altitude, vertical velocity |
| **IMU** | Orientation, angular velocity | | Velocity | Estimated velocity (x, y, z) |
| **Altimeter** | Altitude, vertical velocity | | Camera | 320x240 downward image |
| **Velocity** | Estimated from optical flow | | Landing Pad | Relative position (may be null!) |
| **Camera** | 320x240 downward image (base64 JPEG) |
| **Landing Pad** | Relative position (may be null!) |
## Function to Implement ## Function to Implement
@@ -34,55 +29,20 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry):
## Sensor Data ## Sensor Data
### IMU
```python ```python
imu = telemetry['imu'] # Altitude
roll = imu['orientation']['roll'] altitude = telemetry['altimeter']['altitude']
pitch = imu['orientation']['pitch'] vertical_vel = telemetry['altimeter']['vertical_velocity']
yaw = imu['orientation']['yaw']
angular_vel = imu['angular_velocity'] # {x, y, z}
```
### Altimeter # Velocity
```python vel_x = telemetry['velocity']['x']
altimeter = telemetry['altimeter'] vel_y = telemetry['velocity']['y']
altitude = altimeter['altitude']
vertical_vel = altimeter['vertical_velocity']
```
### Velocity # Landing Pad (may be None!)
```python landing_pad = telemetry.get('landing_pad')
velocity = telemetry['velocity'] # {x, y, z} in m/s if landing_pad:
``` relative_x = landing_pad['relative_x']
relative_y = landing_pad['relative_y']
### 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
``` ```
## Control Output ## Control Output
@@ -112,7 +72,7 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry):
thrust = 0.5 * (0 - altitude) - 0.3 * vertical_vel thrust = 0.5 * (0 - altitude) - 0.3 * vertical_vel
# Horizontal control # Horizontal control
if landing_pad is not None: if landing_pad:
pitch = 0.3 * landing_pad['relative_x'] - 0.2 * vel_x pitch = 0.3 * landing_pad['relative_x'] - 0.2 * vel_x
roll = 0.3 * landing_pad['relative_y'] - 0.2 * vel_y roll = 0.3 * landing_pad['relative_y'] - 0.2 * vel_y
else: else:
@@ -124,84 +84,43 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry):
## Using the Camera ## Using the Camera
You can implement custom vision processing on the camera image:
```python ```python
import cv2 import cv2
import numpy as np import numpy as np
import base64 import base64
def process_camera(telemetry): camera = telemetry.get('camera', {})
camera = telemetry.get('camera', {}) image_b64 = camera.get('image')
image_b64 = camera.get('image')
if not image_b64: if image_b64:
return None
# Decode JPEG
image_bytes = base64.b64decode(image_b64) image_bytes = base64.b64decode(image_b64)
nparr = np.frombuffer(image_bytes, np.uint8) nparr = np.frombuffer(image_bytes, np.uint8)
image = cv2.imdecode(nparr, cv2.IMREAD_COLOR) image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
# Process image...
# 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
``` ```
## 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 ## Testing
```bash ```bash
# Easy - stationary rover # Easy - stationary
python standalone_simulation.py --pattern stationary python standalone_simulation.py --pattern stationary
# Medium - slow circular movement # Medium - circular
python standalone_simulation.py --pattern circular --speed 0.2 python standalone_simulation.py --pattern circular --speed 0.3
# Hard - faster random movement # Hard - random
python standalone_simulation.py --pattern random --speed 0.3 python standalone_simulation.py --pattern random --speed 0.5
# With ROS 2 (Gazebo)
ros2 launch gazebo/launch/drone_landing.launch.py # Terminal 1
python run_gazebo.py --pattern circular # Terminal 2
``` ```
## Configuration ## Configuration
Edit `config.py` to tune controller gains: Edit `config.py`:
```python ```python
CONTROLLER = { CONTROLLER = {
"Kp_z": 0.5, # Altitude proportional gain "Kp_z": 0.5, # Altitude proportional
"Kd_z": 0.3, # Altitude derivative gain "Kd_z": 0.3, # Altitude derivative
"Kp_xy": 0.3, # Horizontal proportional gain "Kp_xy": 0.3, # Horizontal proportional
"Kd_xy": 0.2, # Horizontal derivative gain "Kd_xy": 0.2, # Horizontal derivative
} }
``` ```

167
docs/gazebo.md
View File

@@ -1,159 +1,72 @@
# Gazebo Simulation Guide # 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 - Gazebo:**
**Terminal 1 - Launch Gazebo + Bridge:**
```bash ```bash
source activate.sh
ros2 launch gazebo/launch/drone_landing.launch.py ros2 launch gazebo/launch/drone_landing.launch.py
``` ```
**Terminal 2 - Run Controllers:** **Terminal 2 - Controllers:**
```bash ```bash
source activate.sh 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 ## Command Options
```bash ```bash
python run_gazebo.py --help python run_gazebo.py --pattern circular --speed 0.5
Options: Options:
--pattern stationary, linear, circular, square, random --pattern, -p stationary, linear, circular, square, random
--speed, -s Rover speed in m/s (default: 0.5) --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 --no-rover Disable rover controller
``` ```
## How It Works ## View Camera
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:
```bash ```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 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 ## 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: **Rover doesn't move:**
1. Check that `run_gazebo.py` is running - Check topic: `ros2 topic echo /rover/cmd_vel`
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 **Model not found:**
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:
```bash ```bash
export GZ_SIM_RESOURCE_PATH=$PWD/gazebo/models:$GZ_SIM_RESOURCE_PATH 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 # Installation Guide
Setup instructions for all supported platforms.
## Quick Install ## Quick Install
```bash
# Ubuntu/Debian
./setup/install_ubuntu.sh
source activate.sh
# Test
python standalone_simulation.py
```
## Install Scripts
| Platform | Command | | Platform | Command |
|----------|---------| |----------|---------|
| Ubuntu/Debian | `./setup/install_ubuntu.sh` | | 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` | | Arch Linux | `./setup/install_arch.sh` |
| macOS | `./setup/install_macos.sh` | | macOS | `./setup/install_macos.sh` |
| Windows | `.\setup\install_windows.ps1` | | Windows | `.\setup\install_windows.ps1` |
After installation: ## Platform Support
```bash
source activate.sh # Linux/macOS
. .\activate.ps1 # Windows PowerShell
python standalone_simulation.py | Mode | Ubuntu | macOS | Windows |
``` |------|--------|-------|---------|
| Standalone | ✅ | ✅ | ✅ |
| Gazebo + ROS 2 | ✅ | ❌ | WSL2 |
| ArduPilot SITL | ✅ | ❌ | WSL2 |
--- ---
## Platform Compatibility ## Ubuntu/Debian
| 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)
```bash ```bash
# Run installer
./setup/install_ubuntu.sh ./setup/install_ubuntu.sh
# Activate environment
source activate.sh source activate.sh
# Run simulation
python standalone_simulation.py python standalone_simulation.py
``` ```
**Installs:** **Installs:**
- ROS 2 (Humble or Jazzy based on Ubuntu version) - ROS 2 Humble/Jazzy
- Gazebo (ros-gz) - Gazebo
- Python packages: pybullet, numpy, pillow, pyinstaller, pymavlink - Python packages (pybullet, numpy, opencv, pymavlink)
---
## ArduPilot SITL
For realistic flight controller simulation:
**With ArduPilot SITL (full flight controller):**
```bash ```bash
# Run installer with ArduPilot ./setup/install_ardupilot.sh
./setup/install_ubuntu.sh --with-ardupilot source ~/.bashrc
```
# This will also install: **Installs:**
# - ArduPilot SITL (~15-20 min build) - ArduPilot SITL
# - ArduPilot Gazebo plugin - ardupilot_gz (Gazebo integration)
# - MAVProxy - 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) 1. Install WSL2:
```powershell
```bash wsl --install -d Ubuntu-22.04
# Run installer
./setup/install_arch.sh
# Activate environment
source activate.sh
# Run simulation
python standalone_simulation.py
``` ```
**Installs:** 2. Open Ubuntu and run:
- Python packages: pybullet, numpy, pillow, pyinstaller
- yay (AUR helper)
**Optional ROS 2 (from AUR):**
```bash ```bash
yay -S ros-humble-desktop ./setup/install_ubuntu.sh
yay -S ros-humble-ros-gz source activate.sh
python standalone_simulation.py
``` ```
--- ---
## macOS ## macOS
**Tested on:** macOS 12+ (Monterey, Ventura, Sonoma)
```bash ```bash
# Run installer
./setup/install_macos.sh ./setup/install_macos.sh
# Activate environment
source activate.sh source activate.sh
# Run simulation
python standalone_simulation.py python standalone_simulation.py
``` ```
**Installs:** **Note:** ROS 2 and Gazebo not supported on macOS. Use standalone mode.
- 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.
--- ---
## Windows ## Manual Install
**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
```bash ```bash
# Create virtual environment
python3 -m venv venv python3 -m venv venv
source venv/bin/activate # Linux/macOS source venv/bin/activate
# OR
.\venv\Scripts\Activate.ps1 # Windows
```
### 3. Install Python Packages # Install packages
```bash
pip install -r requirements.txt pip install -r requirements.txt
```
Or manually: # Run
```bash
pip install pybullet numpy pillow pyinstaller
```
### 4. Run Simulation
```bash
python standalone_simulation.py 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 ## Verification
After installation, verify packages:
```bash ```bash
python -c "import pybullet; print('PyBullet OK')" python -c "import pybullet; print('PyBullet OK')"
python -c "import numpy; print('NumPy OK')" python -c "import cv2; print('OpenCV OK')"
python -c "from PIL import Image; print('Pillow OK')"
python -c "from pymavlink import mavutil; print('pymavlink 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 ```json
{ {
@@ -13,25 +13,20 @@ Message formats for GPS-denied drone operation with camera.
} }
``` ```
| Field | Range | Description | | Field | Range | Effect |
|-------|-------|-------------| |-------|-------|--------|
| `thrust` | ±1.0 | Vertical thrust (positive = up) | | thrust | ±1.0 | Up/down |
| `pitch` | ±0.5 | Forward/backward tilt | | pitch | ±0.5 | Forward/back |
| `roll` | ±0.5 | Left/right tilt | | roll | ±0.5 | Left/right |
| `yaw` | ±0.5 | Rotation | | yaw | ±0.5 | Rotation |
--- ## Telemetry
## Drone Telemetry
Published on `/drone/telemetry`. **No GPS position available.**
```json ```json
{ {
"imu": { "imu": {
"orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0}, "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0},
"angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}, "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}
"linear_acceleration": {"x": 0.0, "y": 0.0, "z": 9.81}
}, },
"altimeter": { "altimeter": {
"altitude": 5.0, "altitude": 5.0,
@@ -47,138 +42,30 @@ Published on `/drone/telemetry`. **No GPS position available.**
"camera": { "camera": {
"width": 320, "width": 320,
"height": 240, "height": 240,
"fov": 60.0, "image": "<base64 JPEG>"
"image": "<base64 encoded JPEG>" }
},
"landed": false,
"timestamp": 1.234
} }
``` ```
--- ## 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 ## Decoding Camera
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
```python ```python
import base64 import base64
import cv2 import cv2
import numpy as np import numpy as np
def decode_camera_image_cv2(telemetry): image_b64 = telemetry['camera']['image']
camera = telemetry.get('camera', {}) image_bytes = base64.b64decode(image_b64)
image_b64 = camera.get('image') nparr = np.frombuffer(image_bytes, np.uint8)
image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
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
}
``` ```

155
docs/pybullet.md
View File

@@ -1,163 +1,68 @@
# PyBullet Simulation Guide # 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, Linux:
No ROS 2 required! Works on Windows, macOS, and Linux:
```bash ```bash
source activate.sh # Linux/macOS source activate.sh
. .\activate.ps1 # Windows python standalone_simulation.py --pattern circular
python standalone_simulation.py --pattern circular --speed 0.3
``` ```
### Options ## ROS 2 Mode (2 Terminals)
```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:
**Terminal 1 - Simulator:** **Terminal 1 - Simulator:**
```bash ```bash
source activate.sh
python simulation_host.py python simulation_host.py
``` ```
**Terminal 2 - Controllers:** **Terminal 2 - Controllers:**
```bash ```bash
source activate.sh python run_bridge.py --pattern circular
python run_bridge.py --pattern circular --speed 0.3
``` ```
### 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 ```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):** ## Remote Setup
```bash
python run_bridge.py --host 192.168.1.100 --pattern circular
```
--- **Machine 1:** `python simulation_host.py`
**Machine 2:** `python run_bridge.py --host <IP>`
## Configuration ## Sensors
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
| Sensor | Description | | Sensor | Description |
|--------|-------------| |--------|-------------|
| **IMU** | Orientation (roll, pitch, yaw), angular velocity | | IMU | Orientation, angular velocity |
| **Altimeter** | Altitude above ground, vertical velocity | | Altimeter | Altitude, vertical velocity |
| **Velocity** | Estimated horizontal velocity (x, y, z) | | Velocity | Estimated velocity (x, y, z) |
| **Camera** | 320x240 downward-facing JPEG image | | Camera | 320x240 downward JPEG |
| **Landing Pad** | Vision-based relative position when in camera FOV | | 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 ```python
CONTROLLER = { CONTROLLER = {
"Kp_z": 0.3, "Kp_z": 0.5,
"Kd_z": 0.2, "Kd_z": 0.3,
"Kp_xy": 0.2, "Kp_xy": 0.3,
"Kd_xy": 0.1, "Kd_xy": 0.2,
} }
``` ```
### Camera image not appearing ## Troubleshooting
Install Pillow: **"Cannot connect to X server":**
```bash ```bash
pip install pillow numpy xvfb-run python standalone_simulation.py
``` ```
### Rover not moving (ROS 2 mode) **Drone flies erratically:**
Reduce gains in `config.py`
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
```

109
docs/rover_controller.md
View File

@@ -4,97 +4,58 @@ The RoverController creates a moving landing pad target.
## Usage ## Usage
The rover controller is automatically included when running `controllers.py`:
```bash ```bash
# Stationary rover (default) # Stationary (default)
python controllers.py python standalone_simulation.py --pattern stationary
# Moving rover # Moving
python controllers.py --pattern circular --speed 0.3 python standalone_simulation.py --pattern circular --speed 0.3
``` ```
### Options ## Options
| Option | Short | Default | Description | | Option | Default | Description |
|--------|-------|---------|-------------| |--------|---------|-------------|
| `--pattern` | `-p` | stationary | Movement pattern | | `--pattern, -p` | stationary | Movement pattern |
| `--speed` | `-s` | 0.5 | Speed in m/s | | `--speed, -s` | 0.5 | Speed in m/s |
| `--amplitude` | `-a` | 2.0 | Amplitude in meters | | `--amplitude, -a` | 2.0 | Radius in meters |
## Movement Patterns ## Patterns
### Stationary | Pattern | Description |
```bash |---------|-------------|
python controllers.py --pattern stationary | stationary | Stays at origin |
``` | linear | Oscillates along X-axis |
Rover stays at origin. Best for initial testing. | circular | Circular path |
| square | Square with sharp turns |
### Linear | random | Random positions |
```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)`.
## Difficulty Levels ## Difficulty Levels
| Level | Pattern | Speed | Description | | Level | Pattern | Speed |
|-------|---------|-------|-------------| |-------|---------|-------|
| Beginner | stationary | 0.0 | Static target | | Beginner | stationary | 0.0 |
| Easy | linear | 0.2 | Predictable 1D | | Easy | linear | 0.2 |
| Medium | circular | 0.3 | Smooth 2D | | Medium | circular | 0.3 |
| Hard | random | 0.3 | Unpredictable | | Hard | random | 0.3 |
| Expert | square | 0.5 | Sharp turns | | Expert | square | 0.5 |
## Progressive Testing ## Progressive Testing
Start easy and increase difficulty:
```bash ```bash
# Step 1: Static target # 1. Static target
python controllers.py --pattern stationary python standalone_simulation.py --pattern stationary
# Step 2: Slow linear motion # 2. Slow circular
python controllers.py --pattern linear --speed 0.2 python standalone_simulation.py --pattern circular --speed 0.2
# Step 3: Slow circular motion # 3. Faster circular
python controllers.py --pattern circular --speed 0.2 python standalone_simulation.py --pattern circular --speed 0.4
# Step 4: Faster circular # 4. Random
python controllers.py --pattern circular --speed 0.4 python standalone_simulation.py --pattern random --speed 0.3
# Step 5: Random
python controllers.py --pattern random --speed 0.3
``` ```
## Published Topics ## Note
| Topic | Type | Description | The drone cannot access rover position directly (GPS-denied). It must detect the landing pad visually via the camera.
|-------|------|-------------|
| `/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.

218
drone_controller.py
View File

@@ -1,16 +1,24 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
DroneController - Template for GPS-denied landing logic. 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(). Implement your algorithm in calculate_landing_maneuver().
Uses sensors: IMU, altimeter, camera, and landing pad detection. Uses sensors: IMU, altimeter, camera, and landing pad detection.
""" """
import json import json
import math
from typing import Dict, Any, Optional from typing import Dict, Any, Optional
import rclpy import rclpy
from rclpy.node import Node 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 from std_msgs.msg import String
# Load configuration # Load configuration
@@ -24,10 +32,38 @@ except ImportError:
LANDING = {"height_threshold": 0.1, "success_velocity": 0.1} LANDING = {"height_threshold": 0.1, "success_velocity": 0.1}
class DroneController(Node): def quaternion_to_euler(x: float, y: float, z: float, w: float) -> tuple:
"""Drone controller for GPS-denied landing.""" """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') super().__init__('drone_controller')
# Load from config # Load from config
@@ -42,37 +78,189 @@ class DroneController(Node):
self._landing_height = LANDING.get("height_threshold", 0.1) self._landing_height = LANDING.get("height_threshold", 0.1)
self._landing_velocity = LANDING.get("success_velocity", 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('=' * 50)
self.get_logger().info('Drone Controller Starting (GPS-Denied)...') self.get_logger().info('Drone Controller Starting (GPS-Denied)...')
if CONFIG_LOADED: if CONFIG_LOADED:
self.get_logger().info(' Configuration loaded from config.py') 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) self.get_logger().info('=' * 50)
# State variables
self._latest_telemetry: Optional[Dict[str, Any]] = None self._latest_telemetry: Optional[Dict[str, Any]] = None
self._rover_telemetry: Optional[Dict[str, Any]] = None self._rover_telemetry: Optional[Dict[str, Any]] = None
self._telemetry_received = False self._telemetry_received = False
self._landing_complete = False self._landing_complete = False
self._telemetry_sub = self.create_subscription( # ArduPilot state (built from DDS topics)
String, '/drone/telemetry', self._telemetry_callback, 10 self._ap_pose: Optional[PoseStamped] = None
) self._ap_twist: Optional[TwistStamped] = None
self.get_logger().info(' Subscribed to: /drone/telemetry') 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( self._rover_telemetry_sub = self.create_subscription(
String, '/rover/telemetry', self._rover_telemetry_callback, 10 String, '/rover/telemetry', self._rover_telemetry_callback, 10
) )
self.get_logger().info(' Subscribed to: /rover/telemetry') self.get_logger().info(' Subscribed to: /rover/telemetry')
# Command publisher
self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10) self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.get_logger().info(' Publishing to: /cmd_vel') self.get_logger().info(' Publishing to: /cmd_vel')
# Control loop
control_period = 1.0 / self._control_rate control_period = 1.0 / self._control_rate
self._control_timer = self.create_timer(control_period, self._control_loop) 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(f' Control loop: {self._control_rate} Hz')
self.get_logger().info('Drone Controller Ready!') 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: def _telemetry_callback(self, msg: String) -> None:
try: try:
self._latest_telemetry = json.loads(msg.data) self._latest_telemetry = json.loads(msg.data)
@@ -211,11 +399,19 @@ class DroneController(Node):
def main(args=None): 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 controller = None
try: try:
controller = DroneController() controller = DroneController(use_ardupilot_topics=use_ardupilot)
rclpy.spin(controller) rclpy.spin(controller)
except KeyboardInterrupt: except KeyboardInterrupt:
print('\nShutting down...') print('\nShutting down...')

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

@@ -1,40 +1,52 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
ArduPilot SITL + Gazebo Launch File ArduPilot ROS 2 Launch Helper
Works with Gazebo Harmonic/Garden and ArduPilot SITL.
This launch file: This file provides a helper for launching the official ArduPilot ROS 2 simulation.
1. Starts Gazebo with ArduPilot-compatible drone model For the full ArduPilot + Gazebo experience, use the official ardupilot_gz packages.
2. Sets up ROS-Gazebo bridge for telemetry
3. Optionally starts ArduPilot SITL
NOTE: ArduPilot SITL integration requires the ardupilot_gazebo plugin. RECOMMENDED: Use the official launch files:
Install from: https://github.com/ArduPilot/ardupilot_gazebo ros2 launch ardupilot_gz_bringup iris_runway.launch.py
This local launch file is for custom Gazebo worlds or fallback testing.
""" """
import os import os
import shutil import shutil
from launch import LaunchDescription 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.substitutions import LaunchConfiguration
from launch_ros.actions import Node from launch_ros.actions import Node
def generate_launch_description(): 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 # Get paths
script_dir = os.path.dirname(os.path.abspath(__file__)) script_dir = os.path.dirname(os.path.abspath(__file__))
gazebo_dir = os.path.dirname(script_dir) 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 # Check if official ArduPilot packages are available
plugin_paths = [ try:
os.path.expanduser("~/ardupilot_gazebo/build"), import subprocess
"/opt/ardupilot_gazebo/lib", result = subprocess.run(
os.environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", ""), ['ros2', 'pkg', 'prefix', 'ardupilot_gz_bringup'],
] capture_output=True, text=True
ardupilot_plugin_found = any(os.path.exists(p) for p in plugin_paths if p) )
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 # Determine Gazebo command
if shutil.which('gz'): if shutil.which('gz'):
@@ -44,37 +56,35 @@ def generate_launch_description():
else: else:
gz_cmd = ['gz', 'sim', '-r', world_file] 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 = [ actions = [
DeclareLaunchArgument( DeclareLaunchArgument(
'use_sim_time', 'use_sim_time',
default_value='true', default_value='true',
description='Use simulation clock' description='Use simulation clock'
), ),
]
DeclareLaunchArgument( if official_pkg_available:
'start_sitl', # Use official ArduPilot packages
default_value='false', actions.append(
description='Start ArduPilot SITL automatically' 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( ExecuteProcess(
cmd=gz_cmd, cmd=gz_cmd,
output='screen', output='screen',
additional_env=env
), ),
# ROS-Gazebo Bridge for telemetry and commands # ROS-Gazebo Bridge
# Delayed start to wait for Gazebo
TimerAction( TimerAction(
period=2.0, period=2.0,
actions=[ actions=[
@@ -83,14 +93,13 @@ def generate_launch_description():
executable='parameter_bridge', executable='parameter_bridge',
name='gz_bridge', name='gz_bridge',
arguments=[ arguments=[
# Rover velocity commands (ROS to Gazebo) # Rover velocity commands
'/rover/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist', '/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', '/model/drone/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
# Camera (from Gazebo to ROS) '/model/rover/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
'/drone/camera@sensor_msgs/msg/Image[gz.msgs.Image',
# IMU (from Gazebo to ROS)
'/imu@sensor_msgs/msg/Imu[gz.msgs.IMU',
# Clock # Clock
'/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock', '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
], ],
@@ -99,36 +108,31 @@ def generate_launch_description():
), ),
] ]
), ),
] ])
return LaunchDescription(actions) return LaunchDescription(actions)
if __name__ == '__main__': if __name__ == '__main__':
print("=" * 60) print("=" * 60)
print(" ArduPilot SITL + Gazebo Launch File") print(" ArduPilot ROS 2 Launch Helper")
print("=" * 60) print("=" * 60)
print() print()
print("This is a ROS 2 launch file for ArduPilot integration.") print("RECOMMENDED: Use official ArduPilot ROS 2 packages:")
print() print()
print("Prerequisites:") print(" # Install ArduPilot ROS 2")
print(" 1. Gazebo (gz sim or ign gazebo)") print(" ./setup/install_ardupilot.sh")
print(" 2. ros_gz_bridge package")
print(" 3. ArduPilot Gazebo plugin (optional, for SITL)")
print() 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(" ros2 launch gazebo/launch/ardupilot_drone.launch.py")
print() 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>

820
mavlink_bridge.py
View File

@@ -1,579 +1,423 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
MAVLink Bridge for ArduPilot SITL Integration. MAVLink Command Interface for ArduPilot.
Connects Gazebo simulation to ArduPilot SITL via MAVProxy.
This bridge: This module provides a Python interface for sending MAVLink commands to ArduPilot
- Connects to ArduPilot SITL via MAVLink (UDP) when using the official ROS 2 DDS integration. Telemetry is received via native
- Receives telemetry from ArduPilot (position, attitude, battery, etc.) ROS 2 topics (/ap/*), but commands are still sent via MAVLink.
- Sends commands to ArduPilot (velocity, position, etc.)
- Publishes ROS 2 telemetry for the drone controller 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 time
import threading import threading
from typing import Optional, Dict, Any, Tuple from typing import Optional, Callable
from enum import Enum
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import String
try: try:
from pymavlink import mavutil from pymavlink import mavutil
from pymavlink.dialects.v20 import ardupilotmega as mavlink PYMAVLINK_AVAILABLE = True
MAVLINK_AVAILABLE = True
except ImportError: except ImportError:
MAVLINK_AVAILABLE = False PYMAVLINK_AVAILABLE = False
print("WARNING: pymavlink not installed. Run: pip install pymavlink") 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 def __init__(self, connection_string: str = "udpin:127.0.0.1:14550"):
DEFAULT_SITL_HOST = "127.0.0.1" """
DEFAULT_SITL_PORT = 14550 # MAVProxy output port Initialize MAVLink commander.
DEFAULT_SYSTEM_ID = 1
DEFAULT_COMPONENT_ID = 191 # MAV_COMP_ID_MISSIONPLANNER
# Telemetry rate (Hz) Args:
TELEMETRY_RATE = 50.0 connection_string: MAVLink connection string (default for MAVProxy output)
"""
if not PYMAVLINK_AVAILABLE:
raise ImportError("pymavlink is required: pip install pymavlink")
def __init__( self._connection_string = connection_string
self, self._mav: Optional[mavutil.mavlink_connection] = None
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._connected = False self._connected = False
self._armed = False self._armed = False
self._flight_mode = "UNKNOWN" self._flight_mode = "UNKNOWN"
self._heartbeat_thread: Optional[threading.Thread] = None
self._running = False
# Telemetry state # Callbacks
self._position = [0.0, 0.0, 0.0] # x, y, z (NED frame, converted to ENU) self._on_heartbeat: Optional[Callable] = None
self._velocity = [0.0, 0.0, 0.0] # vx, vy, vz self._on_state_change: Optional[Callable] = None
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
# Rover position (for landing pad detection) def connect(self, timeout: float = 30.0) -> bool:
self._rover_position = [0.0, 0.0, 0.15] """
Connect to ArduPilot via MAVLink.
# Thread safety Args:
self._lock = threading.Lock() timeout: Connection timeout in seconds
# ROS 2 Subscriptions Returns:
self._cmd_vel_sub = self.create_subscription( True if connected successfully
Twist, '/cmd_vel', self._cmd_vel_callback, 10 """
) print(f"Connecting to {self._connection_string}...")
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
try: try:
connection_string = f'udpin:{self._sitl_host}:{self._sitl_port}' self._mav = mavutil.mavlink_connection(self._connection_string)
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
)
# Wait for heartbeat # Wait for heartbeat
self.get_logger().info('Waiting for heartbeat...') heartbeat = self._mav.wait_heartbeat(timeout=timeout)
self._mav_conn.wait_heartbeat(timeout=30) if heartbeat is None:
print("Connection failed: No heartbeat received")
return False
self._connected = True self._connected = True
self._last_heartbeat = time.time() self._running = True
self.get_logger().info(
f'Connected! System: {self._mav_conn.target_system}, ' # Start heartbeat thread
f'Component: {self._mav_conn.target_component}' self._heartbeat_thread = threading.Thread(
target=self._heartbeat_loop, daemon=True
) )
self._heartbeat_thread.start()
# Request data streams print(f"Connected! System: {self._mav.target_system}, Component: {self._mav.target_component}")
self._request_data_streams()
return True return True
except Exception as e: except Exception as e:
self.get_logger().error(f'MAVLink connection failed: {e}') print(f"Connection failed: {e}")
self._connected = False
return False return False
def _request_data_streams(self): def disconnect(self):
"""Request required data streams from ArduPilot.""" """Disconnect from ArduPilot."""
if not self._mav_conn: self._running = False
return if self._heartbeat_thread:
self._heartbeat_thread.join(timeout=2.0)
# Request all data streams at high rate if self._mav:
streams = [ self._mav.close()
(mavutil.mavlink.MAV_DATA_STREAM_ALL, 10), self._connected = False
(mavutil.mavlink.MAV_DATA_STREAM_RAW_SENSORS, 50), print("Disconnected")
(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 _heartbeat_loop(self):
"""Background thread for receiving heartbeats and updating state."""
while self._running and self._mav:
try: try:
# Non-blocking message receive msg = self._mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1.0)
msg = self._mav_conn.recv_match(blocking=True, timeout=0.1) if msg:
self._armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
self._flight_mode = mavutil.mode_string_v10(msg)
if msg is None: if self._on_heartbeat:
# Check for connection timeout self._on_heartbeat(self._armed, self._flight_mode)
if time.time() - self._last_heartbeat > 5.0: except Exception:
self.get_logger().warning('Heartbeat timeout, reconnecting...') pass
self._connected = False
continue
self._process_mavlink_message(msg) @property
def connected(self) -> bool:
return self._connected
except Exception as e: @property
self.get_logger().error(f'MAVLink receive error: {e}') def armed(self) -> bool:
self._connected = False return self._armed
def _process_mavlink_message(self, msg): @property
"""Process incoming MAVLink messages.""" def flight_mode(self) -> str:
msg_type = msg.get_type() return self._flight_mode
with self._lock:
if msg_type == 'HEARTBEAT':
self._last_heartbeat = time.time()
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:
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
dx = self._rover_position[0] - self._position[0]
dy = self._rover_position[1] - self._position[1]
dz = self._rover_position[2] - self._position[2]
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
# =========================================================================
def arm(self, force: bool = False) -> bool: 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 return False
self.get_logger().info('Arming drone...') print("Arming...")
self._mav_conn.arducopter_arm()
return self._wait_for_arm(True, timeout=5.0) 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: 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 return False
self.get_logger().info('Disarming drone...') print("Disarming...")
self._mav_conn.arducopter_disarm()
return self._wait_for_arm(False, timeout=5.0)
def _wait_for_arm(self, expected: bool, timeout: float = 5.0) -> bool: param = 21196.0 if force else 0.0
"""Wait for arm/disarm to complete."""
start = time.time() self._mav.mav.command_long_send(
while time.time() - start < timeout: self._mav.target_system,
if self._armed == expected: self._mav.target_component,
return True mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
time.sleep(0.1) 0,
return False 0, # Disarm
param,
0, 0, 0, 0, 0
)
time.sleep(0.5)
return True
def set_mode(self, mode: str) -> bool: 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 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_id is None:
if mode.upper() not in mode_mapping: print(f"Unknown mode: {mode}")
self.get_logger().error(f'Unknown mode: {mode}')
return False return False
mode_id = mode_mapping[mode.upper()] print(f"Setting mode to {mode_upper}...")
self._mav_conn.set_mode(mode_id)
# Wait for mode change self._mav.mav.set_mode_send(
start = time.time() self._mav.target_system,
while time.time() - start < 5.0: mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
if self._flight_mode.upper() == mode.upper(): mode_id
return True )
time.sleep(0.1)
return False time.sleep(0.5)
return True
def takeoff(self, altitude: float = 5.0) -> bool: 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 return False
self.get_logger().info(f'Taking off to {altitude}m...') print(f"Taking off to {altitude}m...")
# Set GUIDED mode self._mav.mav.command_long_send(
if not self.set_mode('GUIDED'): self._mav.target_system,
self.get_logger().error('Failed to set GUIDED mode') self._mav.target_component,
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,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
0, # confirmation 0,
0, 0, 0, 0, # params 1-4 (unused) 0, 0, 0, 0, 0, 0,
0, 0, # lat, lon (use current) altitude
altitude # altitude
) )
return True return True
def land(self) -> bool: 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 return False
self.get_logger().info('Landing...') print("Landing...")
return self.set_mode('LAND')
def goto_position(self, x: float, y: float, z: float, yaw: float = 0.0): self._mav.mav.command_long_send(
"""Go to a local position (ENU frame).""" self._mav.target_system,
if not self._connected or not self._mav_conn: self._mav.target_component,
return mavutil.mavlink.MAV_CMD_NAV_LAND,
0,
# Convert ENU to NED 0, 0, 0, 0, 0, 0, 0
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_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 0, # time_boot_ms
self._mav_conn.target_system, self._mav.target_system,
self._mav_conn.target_component, self._mav.target_component,
mavutil.mavlink.MAV_FRAME_LOCAL_NED, mavutil.mavlink.MAV_FRAME_LOCAL_NED,
type_mask, 0b0000111111111000, # Position only
y, x, -z, # ENU to NED position north, east, down, # Position
0, 0, 0, # velocity (ignored) 0, 0, 0, # Velocity
0, 0, 0, # acceleration (ignored) 0, 0, 0, # Acceleration
yaw, 0 # yaw, yaw_rate 0, 0 # Yaw, yaw_rate
) )
def destroy_node(self): return True
"""Clean shutdown."""
self._running = False def set_velocity_ned(self, vn: float, ve: float, vd: float, yaw_rate: float = 0.0) -> bool:
if self._mavlink_thread.is_alive(): """
self._mavlink_thread.join(timeout=2.0) Set velocity in NED frame.
if self._mav_conn:
self._mav_conn.close() Args:
super().destroy_node() 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): def main():
if not MAVLINK_AVAILABLE: """Example usage of MAVLinkCommander."""
print("ERROR: pymavlink is required. Install with: pip install pymavlink") if not PYMAVLINK_AVAILABLE:
print("pymavlink not installed")
return return
print("\n" + "=" * 60) cmd = MAVLinkCommander()
print(" MAVLink Bridge for ArduPilot SITL")
print("=" * 60 + "\n")
rclpy.init(args=args) if not cmd.connect(timeout=10.0):
bridge = None print("Failed to connect")
return
try: print(f"Armed: {cmd.armed}")
bridge = MAVLinkBridge() print(f"Mode: {cmd.flight_mode}")
rclpy.spin(bridge)
except KeyboardInterrupt: # Example commands (uncomment to use):
print('\nShutting down...') # cmd.set_mode("GUIDED")
finally: # cmd.arm()
if bridge: # cmd.takeoff(5.0)
bridge.destroy_node() # time.sleep(10)
if rclpy.ok(): # cmd.land()
rclpy.shutdown()
input("Press Enter to disconnect...")
cmd.disconnect()
if __name__ == '__main__': 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 pybullet>=3.2.5
numpy>=1.20.0 numpy>=1.20.0
pillow>=9.0.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 pymavlink>=2.4.0
# Build executables (optional)
pyinstaller>=6.0.0

472
run_ardupilot.py
View File

@@ -1,16 +1,20 @@
#!/usr/bin/env python3 #!/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: This script provides a convenient way to launch the ArduPilot SITL simulation
1. Launches ArduPilot SITL (sim_vehicle.py with Gazebo model) using the official ardupilot_gz packages with ROS 2 DDS support.
2. Starts MAVProxy for GCS connectivity
3. Runs MAVLink bridge + controllers
Usage: Usage:
python run_ardupilot.py # Start everything python run_ardupilot.py # Launch Iris on runway
python run_ardupilot.py --pattern circular # With rover movement python run_ardupilot.py --world maze # Launch Iris in maze
python run_ardupilot.py --no-sitl # Just controllers (SITL already running) 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 import argparse
@@ -21,336 +25,208 @@ import sys
import time import time
from pathlib import Path from pathlib import Path
import rclpy
from rclpy.executors import MultiThreadedExecutor
# Check for pymavlink def check_ros2():
try: """Check if ROS 2 is available."""
from pymavlink import mavutil try:
MAVLINK_AVAILABLE = True subprocess.run(['ros2', '--help'], capture_output=True, check=True)
except ImportError: return True
MAVLINK_AVAILABLE = False except (subprocess.CalledProcessError, FileNotFoundError):
return False
from mavlink_bridge import MAVLinkBridge
from drone_controller import DroneController
from rover_controller import RoverController, MovementPattern
def find_sim_vehicle() -> str: def check_ardupilot_packages():
"""Find sim_vehicle.py in common ArduPilot locations.""" """Check if ArduPilot ROS 2 packages are installed."""
locations = [ try:
os.path.expanduser("~/ardupilot/Tools/autotest/sim_vehicle.py"), result = subprocess.run(
"/opt/ardupilot/Tools/autotest/sim_vehicle.py", ['ros2', 'pkg', 'list'],
os.environ.get("ARDUPILOT_HOME", "") + "/Tools/autotest/sim_vehicle.py", 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: for term_cmd in terminals:
if os.path.exists(loc): try:
return loc subprocess.Popen(term_cmd)
return True
except FileNotFoundError:
continue
# Try finding it in PATH print(f"[WARN] Could not open terminal for MAVProxy")
import shutil print(f"[INFO] Run manually: {mavproxy_cmd}")
if shutil.which("sim_vehicle.py"): return False
return "sim_vehicle.py"
return None
def parse_args(): def parse_args():
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description='Run ArduPilot SITL + Gazebo + MAVLink Bridge', description='Launch ArduPilot SITL with ROS 2 and Gazebo',
formatter_class=argparse.RawDescriptionHelpFormatter, formatter_class=argparse.RawDescriptionHelpFormatter,
epilog=""" epilog="""
Examples: Examples:
python run_ardupilot.py # Full stack with Gazebo python run_ardupilot.py # Iris on runway
python run_ardupilot.py --pattern circular # Moving rover python run_ardupilot.py --world maze # Iris in maze
python run_ardupilot.py --no-sitl # SITL already running python run_ardupilot.py --vehicle rover # WildThumper rover
python run_ardupilot.py --mavproxy-port 14551 # Custom port python run_ardupilot.py --mavproxy # With MAVProxy
Available worlds: runway, maze, sitl (no Gazebo)
Available vehicles: copter, rover
""" """
) )
# SITL options
parser.add_argument( parser.add_argument(
'--no-sitl', action='store_true', '--world', '-w', type=str, default='runway',
help='Skip launching SITL (assume already running)' choices=['runway', 'maze', 'sitl'],
help='Simulation world (default: runway)'
) )
parser.add_argument( parser.add_argument(
'--sitl-path', type=str, '--vehicle', '-v', type=str, default='copter',
help='Path to sim_vehicle.py' choices=['copter', 'rover'],
help='Vehicle type (default: copter)'
) )
parser.add_argument( parser.add_argument(
'--vehicle', type=str, default='ArduCopter', '--mavproxy', '-m', action='store_true',
choices=['ArduCopter', 'ArduPlane', 'APMrover2'], help='Also launch MAVProxy in a new terminal'
help='Vehicle type (default: ArduCopter)'
) )
parser.add_argument(
'--frame', type=str, default='gazebo-iris',
help='Vehicle frame (default: gazebo-iris)'
)
# MAVProxy settings
parser.add_argument( parser.add_argument(
'--mavproxy-port', type=int, default=14550, '--mavproxy-port', type=int, default=14550,
help='MAVProxy output port (default: 14550)' help='MAVProxy master port (default: 14550)'
)
parser.add_argument(
'--sitl-port', type=int, default=5760,
help='SITL connection port (default: 5760)'
) )
# Rover options return parser.parse_args()
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
def main(): def main():
args = parse_args() args = parse_args()
if not MAVLINK_AVAILABLE:
print("ERROR: pymavlink is required!")
print("Install with: pip install pymavlink")
return 1
print("=" * 60) print("=" * 60)
print(" ArduPilot SITL + Gazebo Simulation") print(" ArduPilot SITL + Gazebo (Official ROS 2 DDS)")
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("=" * 60)
print() print()
sitl = None # Check ROS 2
if not check_ros2():
# Start SITL if not skipped print("[ERROR] ROS 2 not found!")
if not args.no_sitl: print("Please source ROS 2:")
sim_vehicle = args.sitl_path or find_sim_vehicle() print(" source /opt/ros/humble/setup.bash")
if not sim_vehicle:
print("=" * 60)
print("WARNING: sim_vehicle.py not found!")
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():
return 1
print("\nWaiting for SITL to initialize...")
time.sleep(5) # Give SITL time to start
# 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 return 1
# Initialize ROS print("[OK] ROS 2 available")
rclpy.init()
nodes = [] # Check ArduPilot packages
executor = MultiThreadedExecutor(num_threads=4) if not check_ardupilot_packages():
print("[ERROR] ArduPilot ROS 2 packages not found!")
print()
print("Please install ArduPilot ROS 2:")
print(" ./setup/install_ardupilot.sh")
print()
print("Then source the workspace:")
print(" source ~/ardu_ws/install/setup.bash")
return 1
print("[OK] ArduPilot ROS 2 packages found")
# Get launch command
launch_cmd = get_launch_command(args.world, args.vehicle)
print()
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 gracefully
def signal_handler(sig, frame):
print("\nShutting down...")
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
# Run the launch command
try: try:
# Create MAVLink bridge subprocess.run(launch_cmd, check=True)
bridge = MAVLinkBridge( except subprocess.CalledProcessError as e:
sitl_port=args.mavproxy_port print(f"[ERROR] Launch failed: {e}")
) return 1
nodes.append(bridge) except KeyboardInterrupt:
executor.add_node(bridge) print("\nShutdown complete.")
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")
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()
print("Press Ctrl+C to stop.")
print()
# Handle Ctrl+C
def signal_handler(sig, frame):
print("\nShutting down...")
executor.shutdown()
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.")
return 0 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 # Drone Simulation - Ubuntu/Debian Installation Script
# ============================================================================= # =============================================================================
# Installs ROS 2, Gazebo, PyBullet, and all required dependencies # 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 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" 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 if [ "$INSTALL_ARDUPILOT" = true ]; then
echo "" echo ""
echo "[STEP 9] Installing ArduPilot SITL..." echo "[STEP 9] Installing ArduPilot SITL..."
echo "[INFO] Calling dedicated ArduPilot install script..."
# Install ArduPilot dependencies # Call the dedicated ArduPilot install script
sudo apt-get install -y \ if [ -f "$SCRIPT_DIR/install_ardupilot.sh" ]; then
python3-dev \ bash "$SCRIPT_DIR/install_ardupilot.sh"
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"
else 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 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 fi
echo "" echo ""