From 6804180e21bdc0350a1f26a132baadeef9627a24 Mon Sep 17 00:00:00 2001 From: default Date: Sun, 4 Jan 2026 00:24:46 +0000 Subject: [PATCH] ArduPilot SITL Update --- README.md | 159 ++--- build_exe.py | 103 ++- camera_viewer.py | 420 ++++++++++++ config.py | 61 +- docs/architecture.md | 206 ++---- docs/ardupilot.md | 286 ++------ docs/drone_guide.md | 157 ++--- docs/gazebo.md | 167 ++--- docs/installation.md | 496 ++------------ docs/protocol.md | 165 +---- docs/pybullet.md | 155 +---- docs/rover_controller.md | 109 +-- drone_controller.py | 220 +++++- gazebo/launch/ardupilot_drone.launch.py | 134 ++-- gazebo/worlds/ardupilot_drone.sdf | 504 -------------- mavlink_bridge.py | 868 ++++++++++-------------- requirements.txt | 13 +- run_ardupilot.py | 472 +++++-------- setup/install_ardupilot.sh | 351 ++++++++++ setup/install_ubuntu.sh | 62 +- 20 files changed, 2138 insertions(+), 2970 deletions(-) create mode 100644 camera_viewer.py delete mode 100644 gazebo/worlds/ardupilot_drone.sdf create mode 100755 setup/install_ardupilot.sh diff --git a/README.md b/README.md index eecaded..9de988d 100644 --- a/README.md +++ b/README.md @@ -1,154 +1,107 @@ # Drone Landing Simulation (GPS-Denied) -A GPS-denied drone landing simulation using relative sensors (IMU, altimeter, camera, landing pad detection) with multiple backends: -- **PyBullet** - Lightweight physics simulation -- **Gazebo** - Full robotics simulator -- **ArduPilot SITL** - Realistic flight controller with MAVProxy +A GPS-denied drone landing simulation with multiple backends. Land a drone on a moving platform using only relative sensors (IMU, altimeter, camera). ## Quick Start -### Standalone Mode (Any Platform - No ROS 2 Required) +### Standalone Mode (No ROS 2 - Any Platform) ```bash -source activate.sh # Linux/macOS -. .\activate.ps1 # Windows - +source activate.sh python standalone_simulation.py --pattern circular --speed 0.3 ``` -### PyBullet + ROS 2 (Two Terminals) +### Gazebo + ROS 2 (Linux/WSL2) -**Terminal 1 - Simulator:** -```bash -python simulation_host.py -``` - -**Terminal 2 - Bridge + Controllers:** -```bash -python run_bridge.py --pattern circular --speed 0.3 -``` - -### Gazebo + ROS 2 (Two Terminals - Linux/WSL2) - -**Terminal 1 - Launch Gazebo + Bridge:** +**Terminal 1:** ```bash ros2 launch gazebo/launch/drone_landing.launch.py ``` -**Terminal 2 - Run Controllers:** +**Terminal 2:** ```bash -python run_gazebo.py --pattern circular --speed 0.3 +source activate.sh +python run_gazebo.py --pattern circular +python camera_viewer.py # Optional: view camera feed ``` -### ArduPilot SITL + Gazebo (Three Terminals - Realistic Flight Controller) +### ArduPilot SITL (Realistic Flight Controller) -**Terminal 1 - Launch Gazebo with ArduPilot world:** +**Terminal 1:** ```bash -ros2 launch gazebo/launch/ardupilot_drone.launch.py +source ~/ardu_ws/install/setup.bash +ros2 launch ardupilot_gz_bringup iris_runway.launch.py ``` -**Terminal 2 - Start ArduPilot SITL:** +**Terminal 2:** ```bash -cd ~/ardupilot -sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console -``` - -**Terminal 3 - Run MAVLink Bridge + Controllers:** -```bash -python run_ardupilot.py --no-sitl --pattern circular +mavproxy.py --console --map --master=:14550 ``` ## Installation -| Platform | Command | -|----------|---------| +```bash +# Ubuntu/Debian (includes ROS 2 + Gazebo) +./setup/install_ubuntu.sh + +# With ArduPilot SITL +./setup/install_ardupilot.sh + +# Activate environment +source activate.sh +``` + +| Platform | Install Script | +|----------|---------------| | Ubuntu/Debian | `./setup/install_ubuntu.sh` | +| ArduPilot SITL | `./setup/install_ardupilot.sh` | | Arch Linux | `./setup/install_arch.sh` | | macOS | `./setup/install_macos.sh` | | Windows | `.\setup\install_windows.ps1` | -## Platform Compatibility - -| Feature | Ubuntu | Arch | macOS | Windows | WSL2 | -|---------|--------|------|-------|---------|------| -| Standalone | ✅ | ✅ | ✅ | ✅ | ✅ | -| PyBullet + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ | -| Gazebo + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ | -| ArduPilot + Gazebo | ✅ | ⚠️ | ❌ | ❌ | ✅ | - ## Files | File | Description | |------|-------------| -| `standalone_simulation.py` | All-in-one (no ROS 2 required) | -| `simulation_host.py` | PyBullet simulator server | -| `run_bridge.py` | PyBullet bridge + Controllers | -| `run_gazebo.py` | Gazebo bridge + Controllers | -| `run_ardupilot.py` | **ArduPilot SITL** + MAVLink bridge | -| `mavlink_bridge.py` | MAVLink ↔ ROS 2 bridge | -| `config.py` | **Configuration file** (edit to customize) | -| `drone_controller.py` | **Your landing algorithm** (edit this!) | -| `rover_controller.py` | Moving landing pad controller | +| `standalone_simulation.py` | **All-in-one** (no ROS 2) | +| `run_gazebo.py` | Gazebo controllers | +| `run_ardupilot.py` | ArduPilot launcher | +| `camera_viewer.py` | Drone camera window | +| `drone_controller.py` | **Your landing algorithm** | +| `config.py` | Configuration | + +## Sensors Available + +| Sensor | Data | +|--------|------| +| IMU | Orientation (roll, pitch, yaw), angular velocity | +| Altimeter | Altitude, vertical velocity | +| Velocity | Estimated velocity (x, y, z) | +| Camera | Downward-facing image | +| Landing Pad | Relative position when visible | ## Configuration Edit `config.py` to customize: -- Drone/rover starting positions -- Physical properties (mass, size) +- Drone/rover positions - Controller gains (Kp, Kd) -- Landing detection thresholds +- Camera settings +- Landing thresholds ## Command Line Options ```bash -# Standalone (no ROS 2) -python standalone_simulation.py --pattern circular --speed 0.3 - -# PyBullet + ROS 2 -python run_bridge.py --pattern circular --speed 0.3 --host - -# Gazebo + ROS 2 -python run_gazebo.py --pattern circular --speed 0.3 - -Options: - --pattern, -p stationary, linear, circular, square, random - --speed, -s Speed in m/s (default: 0.5) - --amplitude, -a Amplitude in meters (default: 2.0) - --host, -H Simulator IP (default: 0.0.0.0) +# Movement patterns +--pattern, -p stationary, linear, circular, square, random +--speed, -s Speed in m/s (default: 0.5) +--amplitude, -a Amplitude in meters (default: 2.0) ``` -## GPS-Denied Sensors - -| Sensor | Data | -|--------|------| -| **IMU** | Orientation (roll, pitch, yaw), angular velocity | -| **Altimeter** | Altitude, vertical velocity | -| **Velocity** | Estimated horizontal velocity (x, y, z) | -| **Camera** | 320x240 downward-facing JPEG image | -| **Landing Pad** | Relative position when visible in camera FOV | - ## Documentation | Document | Description | |----------|-------------| -| [Installation](docs/installation.md) | Platform setup guides + WSL2 | -| [Architecture](docs/architecture.md) | System components diagram | -| [Gazebo Guide](docs/gazebo.md) | Gazebo-specific instructions | -| [PyBullet Guide](docs/pybullet.md) | PyBullet-specific instructions | -| [ArduPilot Guide](docs/ardupilot.md) | **ArduPilot SITL + MAVProxy** | -| [Protocol](docs/protocol.md) | Sensor data formats | -| [Drone Guide](docs/drone_guide.md) | Landing algorithm guide | - -## Network Setup (Remote Simulator) - -Run simulator on one machine, controllers on another: - -**Machine 1 (with display):** -```bash -python simulation_host.py # Listens on 0.0.0.0:5555 -``` - -**Machine 2 (headless):** -```bash -python run_bridge.py --host 192.168.1.100 # Connect to Machine 1 -``` \ No newline at end of file +| [Installation](docs/installation.md) | Full setup guide | +| [Architecture](docs/architecture.md) | System overview | +| [ArduPilot](docs/ardupilot.md) | ArduPilot SITL guide | +| [Gazebo](docs/gazebo.md) | Gazebo guide | \ No newline at end of file diff --git a/build_exe.py b/build_exe.py index b22b254..763f2cd 100644 --- a/build_exe.py +++ b/build_exe.py @@ -7,6 +7,8 @@ Usage: python build_exe.py # Build standalone_simulation python build_exe.py simulation_host # Build simulation_host python build_exe.py standalone # Build standalone_simulation + python build_exe.py ardupilot # Build ArduPilot launcher + python build_exe.py camera_viewer # Build camera feed viewer python build_exe.py all # Build all """ @@ -24,12 +26,25 @@ except ImportError as e: print("Install with: pip install pyinstaller pybullet") sys.exit(1) +# Check for pymavlink (optional for ArduPilot builds) +try: + from pymavlink import mavutil + PYMAVLINK_AVAILABLE = True +except ImportError: + PYMAVLINK_AVAILABLE = False + def get_pybullet_data_path() -> str: return pybullet_data.getDataPath() -def build_executable(source_name: str, output_name: str, console: bool = True): +def build_executable( + source_name: str, + output_name: str, + console: bool = True, + hidden_imports: list = None, + collect_data: list = None +): """Build a single executable.""" script_dir = Path(__file__).parent source_file = script_dir / source_name @@ -59,6 +74,16 @@ def build_executable(source_name: str, output_name: str, console: bool = True): f'--add-data={data_spec}', ] + # Add hidden imports if specified + if hidden_imports: + for imp in hidden_imports: + build_args.append(f'--hidden-import={imp}') + + # Add data collection for packages + if collect_data: + for pkg in collect_data: + build_args.append(f'--collect-data={pkg}') + if console: build_args.append('--console') else: @@ -92,7 +117,7 @@ def main(): 'target', nargs='?', default='standalone', - choices=['standalone', 'simulation_host', 'all'], + choices=['standalone', 'simulation_host', 'ardupilot', 'mavlink_bridge', 'camera_viewer', 'all'], help='What to build (default: standalone)' ) args = parser.parse_args() @@ -102,9 +127,11 @@ def main(): print("=" * 60) print(f"Platform: {platform.system()}") print(f"PyBullet data: {get_pybullet_data_path()}") + print(f"pymavlink: {'Available' if PYMAVLINK_AVAILABLE else 'Not installed'}") success = True + # Build standalone simulation if args.target in ['standalone', 'all']: success &= build_executable( 'standalone_simulation.py', @@ -112,6 +139,7 @@ def main(): console=False ) + # Build simulation host if args.target in ['simulation_host', 'all']: success &= build_executable( 'simulation_host.py', @@ -119,11 +147,81 @@ def main(): console=True ) + # Build MAVLink bridge (requires pymavlink) + if args.target in ['mavlink_bridge', 'all']: + if not PYMAVLINK_AVAILABLE: + print("\nWarning: pymavlink not installed, skipping mavlink_bridge build") + print("Install with: pip install pymavlink") + if args.target == 'mavlink_bridge': + success = False + else: + success &= build_executable( + 'mavlink_bridge.py', + 'mavlink_bridge', + console=True, + hidden_imports=[ + 'pymavlink', + 'pymavlink.mavutil', + 'pymavlink.dialects.v20.ardupilotmega', + ], + collect_data=['pymavlink'] + ) + + # Build ArduPilot runner (requires pymavlink) + if args.target in ['ardupilot', 'all']: + if not PYMAVLINK_AVAILABLE: + print("\nWarning: pymavlink not installed, skipping ardupilot build") + print("Install with: pip install pymavlink") + if args.target == 'ardupilot': + success = False + else: + success &= build_executable( + 'run_ardupilot.py', + 'run_ardupilot', + console=True, + hidden_imports=[ + 'pymavlink', + 'pymavlink.mavutil', + 'pymavlink.dialects.v20.ardupilotmega', + 'mavlink_bridge', + 'drone_controller', + 'rover_controller', + ], + collect_data=['pymavlink'] + ) + + # Build camera viewer (requires opencv) + if args.target in ['camera_viewer', 'all']: + try: + import cv2 + success &= build_executable( + 'camera_viewer.py', + 'camera_viewer', + console=True, + hidden_imports=[ + 'cv2', + 'numpy', + ], + collect_data=['cv2'] + ) + except ImportError: + print("\nWarning: opencv-python not installed, skipping camera_viewer build") + print("Install with: pip install opencv-python") + if args.target == 'camera_viewer': + success = False + print() print("=" * 60) if success: print(" BUILD COMPLETE!") print(" Executables in: dist/") + print() + print(" Available executables:") + dist_dir = Path(__file__).parent / "dist" + if dist_dir.exists(): + for exe in dist_dir.iterdir(): + if exe.is_file() and not exe.name.startswith('.'): + print(f" - {exe.name}") else: print(" BUILD FAILED!") sys.exit(1) @@ -132,3 +230,4 @@ def main(): if __name__ == '__main__': main() + diff --git a/camera_viewer.py b/camera_viewer.py new file mode 100644 index 0000000..207867b --- /dev/null +++ b/camera_viewer.py @@ -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() diff --git a/config.py b/config.py index 7055a6d..582b205 100644 --- a/config.py +++ b/config.py @@ -47,12 +47,19 @@ ROVER = { # ============================================================================= CAMERA = { + # Camera sensor settings (for simulation) "width": 320, "height": 240, "fov": 60.0, # Field of view in degrees "near_clip": 0.1, # Near clipping plane "far_clip": 100.0, # Far clipping plane "jpeg_quality": 70, # JPEG compression quality (1-100) + + # Camera viewer settings (for camera_viewer.py) + "viewer_fps": 30, # Display framerate + "viewer_topic": "/drone/camera", # ROS 2 topic for Gazebo + "viewer_topic_ardupilot": "/camera/image_raw", # Topic for ArduPilot + "show_info_overlay": True, # Show FPS and frame count } # ============================================================================= @@ -103,47 +110,57 @@ CONTROLLER = { } # ============================================================================= -# ARDUPILOT / MAVLINK CONFIGURATION +# ARDUPILOT ROS 2 CONFIGURATION # ============================================================================= ARDUPILOT = { # Vehicle type "vehicle": "ArduCopter", # ArduCopter, ArduPlane, APMrover2 - "frame": "gazebo-iris", # Gazebo model frame + "frame": "iris", # Gazebo model (iris, wildthumper) - # SITL connection - "sitl_host": "127.0.0.1", # SITL host address - "sitl_port": 5760, # SITL TCP port + # ROS 2 workspace (set by install_ardupilot.sh) + "workspace": "~/ardu_ws", - # MAVProxy output (for MAVLink bridge) - "mavproxy_host": "127.0.0.1", - "mavproxy_port": 14550, # MAVProxy UDP output + # Simulation worlds + "default_world": "runway", # runway, maze, sitl - # Gazebo plugin connection (JSON interface) - "gazebo_fdm_port_in": 9002, # Port for motor commands - "gazebo_fdm_port_out": 9003, # Port for sensor data + # MAVProxy connection (for GCS features) + "mavproxy_port": 14550, # MAVProxy UDP port - # Arming requirements (for simulation, can be relaxed) - "require_gps": False, # GPS required for arming - "require_ekf": True, # EKF required for arming + # DDS configuration + "dds_enable": True, # Enable DDS for native ROS 2 topics + "dds_domain_id": 0, # Must match ROS_DOMAIN_ID } +# ArduPilot ROS 2 Topics (read-only reference) +ARDUPILOT_TOPICS = { + # State topics (subscribe) + "pose": "/ap/pose/filtered", # PoseStamped + "geopose": "/ap/geopose/filtered", # GeoPoseStamped + "twist": "/ap/twist/filtered", # TwistStamped + "imu": "/ap/imu/filtered", # Imu + "battery": "/ap/battery", # BatteryState + "navsat": "/ap/navsat", # NavSatFix + + # Command topics (publish) - via MAVLink + "cmd_vel": "/cmd_vel", # Twist (velocity commands) +} + +# MAVLink configuration (for pymavlink/mavproxy) MAVLINK = { # MAVLink system IDs "system_id": 1, # Our system ID "component_id": 191, # MAV_COMP_ID_MISSIONPLANNER - # Target system (usually the autopilot) + # Target system (the autopilot) "target_system": 1, "target_component": 1, - # Connection timeout (seconds) + # Connection + "connection_string": "udpin:127.0.0.1:14550", + + # Timeouts (seconds) "heartbeat_timeout": 5.0, "connection_timeout": 30.0, - - # Data stream rates (Hz) - "stream_rate_position": 50, - "stream_rate_attitude": 50, - "stream_rate_raw_sensors": 50, - "stream_rate_extended_status": 5, } + diff --git a/docs/architecture.md b/docs/architecture.md index 03060dc..7e2e4a8 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -1,176 +1,94 @@ # Architecture Overview -GPS-denied drone landing simulation with multiple operation modes. - ## Operation Modes -### 1. Standalone Mode (Any Platform) +### 1. Standalone (Any Platform) -Single-process simulation - no ROS 2 or networking required: +Single process, no ROS 2 required: + +```bash +python standalone_simulation.py --pattern circular +``` ``` ┌────────────────────────────────────────┐ │ standalone_simulation.py │ │ ┌──────────────────────────────────┐ │ │ │ PyBullet Physics + Camera │ │ -│ │ Built-in Landing Controller │ │ -│ │ Rover Movement Patterns │ │ -│ │ Configuration from config.py │ │ +│ │ Built-in Controller │ │ +│ │ Rover Movement │ │ │ └──────────────────────────────────┘ │ └────────────────────────────────────────┘ ``` -### 2. PyBullet + ROS 2 Mode (Two Terminals) +### 2. Gazebo + ROS 2 (2 Terminals) + +**Terminal 1:** +```bash +ros2 launch gazebo/launch/drone_landing.launch.py +``` + +**Terminal 2:** +```bash +python run_gazebo.py --pattern circular +``` ``` Terminal 1 Terminal 2 -┌──────────────────┐ ┌──────────────────────────┐ -│ simulation_host │◄─UDP───►│ run_bridge.py │ -│ (PyBullet) │ │ ┌────────────────────┐ │ -│ Port 5555 │ │ │ ROS2SimulatorBridge│ │ -│ │ │ │ DroneController │ │ -│ │ │ │ RoverController │ │ -└──────────────────┘ │ └────────────────────┘ │ - └──────────────────────────┘ +┌───────────────────┐ ┌───────────────────┐ +│ Gazebo + Bridge │◄──────►│ run_gazebo.py │ +│ (Physics) │ ROS │ + Controllers │ +└───────────────────┘ └───────────────────┘ ``` -Data flow: -- RoverController publishes position → Bridge sends to Simulator -- Simulator moves rover visually AND sends back telemetry -- DroneController receives telemetry, publishes commands -- Bridge forwards commands to Simulator +### 3. ArduPilot SITL (2 Terminals) -### 3. Gazebo + ROS 2 Mode (Two Terminals, Linux/WSL2) - -``` -Terminal 1 Terminal 2 -┌───────────────────────────┐ ┌──────────────────────────┐ -│ ros2 launch ... .launch.py│ │ run_gazebo.py │ -│ ┌─────────────────────┐ │ │ ┌────────────────────┐ │ -│ │ Gazebo (ign gazebo) │ │ │ │ GazeboBridge │ │ -│ │ - Drone (vel ctrl) │ │◄────►│ │ DroneController │ │ -│ │ - Rover (vel ctrl) │ │ ROS │ │ RoverController │ │ -│ ├─────────────────────┤ │ │ └────────────────────┘ │ -│ │ ros_gz_bridge │ │ └──────────────────────────┘ -│ └─────────────────────┘ │ -└───────────────────────────┘ +**Terminal 1:** +```bash +ros2 launch ardupilot_gz_bringup iris_runway.launch.py ``` -Data flow: -- RoverController publishes to `/rover/cmd_vel` → Gazebo moves rover -- Gazebo publishes odometry → GazeboBridge converts to telemetry -- DroneController receives telemetry, publishes to `/cmd_vel` -- GazeboBridge forwards to `/drone/cmd_vel` → Gazebo moves drone - -### 4. ArduPilot SITL + Gazebo Mode (Three Terminals, Linux/WSL2) - -``` -Terminal 1 Terminal 2 Terminal 3 -┌──────────────┐ ┌─────────────────┐ ┌────────────────────────┐ -│ Gazebo + │ │ ArduPilot SITL │ │ run_ardupilot.py │ -│ ArduPilot │◄──►│ sim_vehicle.py │ │ ┌──────────────────┐ │ -│ Plugin │JSON│ + MAVProxy │◄───►│ │ MAVLinkBridge │ │ -│ │ │ │ UDP │ │ DroneController │ │ -│ ardupilot_ │ │ Flight Control │ │ │ RoverController │ │ -│ drone.sdf │ │ + GCS │ │ └──────────────────┘ │ -└──────────────┘ └─────────────────┘ └────────────────────────┘ +**Terminal 2:** +```bash +mavproxy.py --console --map --master=:14550 ``` -Data flow: -- ArduPilot SITL sends motor commands → Gazebo plugin controls drone -- Gazebo plugin sends sensor data → ArduPilot SITL for state estimation -- MAVProxy outputs telemetry → MAVLinkBridge converts to ROS telemetry -- DroneController receives telemetry, publishes velocity commands -- MAVLinkBridge sends MAVLink commands → ArduPilot SITL executes +``` +┌─────────────────────────────────────────────┐ +│ Single Launch Command │ +│ (Starts SITL + Gazebo + RViz) │ +├─────────────────────────────────────────────┤ +│ ArduPilot SITL ◄──► Gazebo ◄──► ROS 2 │ +│ ▲ │ +│ │ /ap/* topics │ +│ ▼ │ +│ MAVProxy (GCS) │ +└─────────────────────────────────────────────┘ +``` -Key differences from simple Gazebo mode: -- Full ArduPilot flight controller (EKF, stabilization, failsafes) -- Real MAVLink protocol for commands and telemetry -- Support for all ArduPilot flight modes (GUIDED, LAND, etc.) -- Arming checks and safety features -- Compatible with ground control stations (QGroundControl, Mission Planner) +## Key Components -## Components - -| File | Description | -|------|-------------| -| `config.py` | Central configuration (positions, physics, gains) | -| `standalone_simulation.py` | All-in-one simulation | -| `simulation_host.py` | PyBullet physics server (UDP) | -| `run_bridge.py` | PyBullet bridge + controllers | -| `run_gazebo.py` | Gazebo bridge + controllers | -| `run_ardupilot.py` | **ArduPilot SITL** + MAVLink bridge | -| `mavlink_bridge.py` | MAVLink ↔ ROS 2 bridge | -| `drone_controller.py` | **Your landing algorithm** | -| `rover_controller.py` | Moving landing pad | -| `ros_bridge.py` | ROS-UDP bridge (used by run_bridge.py) | -| `gazebo_bridge.py` | Gazebo-ROS bridge (used by run_gazebo.py) | -| `gazebo/launch/drone_landing.launch.py` | ROS 2 launch file for Gazebo | -| `gazebo/launch/ardupilot_drone.launch.py` | ROS 2 launch file for ArduPilot | -| `gazebo/worlds/drone_landing.sdf` | Gazebo world with simple velocity control | -| `gazebo/worlds/ardupilot_drone.sdf` | Gazebo world with ArduPilot plugin | +| Component | Description | +|-----------|-------------| +| `drone_controller.py` | Your landing algorithm | +| `gazebo_bridge.py` | Gazebo ↔ ROS bridge | +| `mavlink_bridge.py` | MAVLink commands | +| `camera_viewer.py` | Camera display | ## ROS 2 Topics -| Topic | Type | Description | -|-------|------|-------------| -| `/cmd_vel` | `Twist` | Drone commands from DroneController | -| `/drone/cmd_vel` | `Twist` | Drone commands to Gazebo | -| `/drone/telemetry` | `String` | GPS-denied sensor data (JSON) | -| `/rover/cmd_vel` | `Twist` | Rover velocity to simulator | -| `/rover/telemetry` | `String` | Rover position (JSON) | +| Topic | Direction | Description | +|-------|-----------|-------------| +| `/drone/telemetry` | ← | Sensor data (JSON) | +| `/cmd_vel` | → | Velocity commands | +| `/drone/camera` | ← | Camera images | +| `/rover/telemetry` | ← | Landing pad position | -## Network Configuration +## ArduPilot Topics -All components default to `0.0.0.0` for network accessibility. - -### Remote Setup (PyBullet mode) - -**Machine 1 (with display):** -```bash -python simulation_host.py # Listens on 0.0.0.0:5555 -``` - -**Machine 2 (headless controller):** -```bash -python run_bridge.py --host 192.168.1.100 -``` - -### UDP Ports - -| Port | Direction | Content | -|------|-----------|---------| -| 5555 | Bridge → Simulator | Commands (JSON) | -| 5556 | Simulator → Bridge | Telemetry (JSON) | - -## GPS-Denied Sensors - -All modes provide the same sensor data: - -| Sensor | Data | -|--------|------| -| **IMU** | Orientation (roll, pitch, yaw), angular velocity | -| **Altimeter** | Altitude above ground, vertical velocity | -| **Velocity** | Estimated velocity (x, y, z) | -| **Camera** | 320x240 downward JPEG (base64) | -| **Landing Pad** | Relative position (x, y, distance) when visible | - -## Configuration (config.py) - -| Section | Parameters | -|---------|------------| -| `DRONE` | mass, size, color, start_position, thrust/torque scales | -| `ROVER` | size, color, start_position, default_pattern, default_speed | -| `CAMERA` | width, height, fov, jpeg_quality | -| `PHYSICS` | gravity, timestep, telemetry_rate | -| `CONTROLLER` | Kp_z, Kd_z, Kp_xy, Kd_xy, rate | -| `LANDING` | success_distance, success_velocity, height_threshold | -| `NETWORK` | host, command_port, telemetry_port | - -## Platform Support - -| Mode | Ubuntu | Arch | macOS | Windows | WSL2 | -|------|--------|------|-------|---------|------| -| Standalone | ✅ | ✅ | ✅ | ✅ | ✅ | -| PyBullet+ROS | ✅ | ⚠️ | ❌ | ❌ | ✅ | -| Gazebo+ROS | ✅ | ⚠️ | ❌ | ❌ | ✅ | +| Topic | Type | +|-------|------| +| `/ap/pose/filtered` | Position | +| `/ap/twist/filtered` | Velocity | +| `/ap/imu/filtered` | IMU | +| `/ap/battery` | Battery | diff --git a/docs/ardupilot.md b/docs/ardupilot.md index 922c369..1d210d2 100644 --- a/docs/ardupilot.md +++ b/docs/ardupilot.md @@ -1,285 +1,101 @@ -# ArduPilot SITL + Gazebo Integration +# ArduPilot SITL Integration -This guide explains how to run the drone simulation with ArduPilot Software-In-The-Loop (SITL) and MAVProxy, providing a realistic flight controller stack. - -## Overview - -The ArduPilot integration replaces the simple velocity control with a full ArduPilot flight stack: - -``` -┌──────────────────┐ ┌─────────────────┐ ┌──────────────────┐ -│ ArduPilot SITL │◄───►│ Gazebo + Plugin │◄───►│ MAVLink Bridge │ -│ (Flight Control)│ JSON│ (Physics Sim) │ ROS │ + Controllers │ -└──────────────────┘ └─────────────────┘ └──────────────────┘ - ▲ │ - │ UDP │ - │ ▼ -┌──────────────────┐ ┌──────────────────┐ -│ MAVProxy │◄────────────────────────────►│ DroneController │ -│ (GCS) │ MAVLink Commands │ (Your Algorithm) │ -└──────────────────┘ └──────────────────┘ -``` - -## Components - -| Component | Description | -|-----------|-------------| -| **ArduPilot SITL** | Full autopilot firmware running in simulation | -| **ardupilot_gazebo** | Plugin connecting Gazebo physics to ArduPilot | -| **MAVProxy** | Ground Control Station for monitoring/commands | -| **MAVLink Bridge** | ROS 2 node bridging MAVLink ↔ ROS topics | -| **Drone Controller** | Your landing algorithm | - -## Prerequisites - -### 1. ArduPilot SITL - -Install ArduPilot development environment: - -```bash -# Ubuntu/Debian -git clone https://github.com/ArduPilot/ardupilot.git ~/ardupilot -cd ~/ardupilot -git submodule update --init --recursive -Tools/environment_install/install-prereqs-ubuntu.sh -y -. ~/.profile - -# Set environment -echo 'export PATH=$PATH:$HOME/ardupilot/Tools/autotest' >> ~/.bashrc -echo 'export ARDUPILOT_HOME=$HOME/ardupilot' >> ~/.bashrc -source ~/.bashrc -``` - -### 2. ArduPilot Gazebo Plugin - -Install the ardupilot_gazebo plugin: - -```bash -# For Gazebo Garden/Harmonic -git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo -cd ~/ardupilot_gazebo -mkdir build && cd build -cmake .. -DCMAKE_BUILD_TYPE=Release -make -j4 - -# Add to Gazebo plugin path -echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH' >> ~/.bashrc -echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH' >> ~/.bashrc -source ~/.bashrc -``` - -### 3. pymavlink - -```bash -pip install pymavlink -``` +Run the simulation with a realistic ArduPilot flight controller. ## Quick Start -### Option 1: Integrated Launch (Recommended) - -This starts everything together: - +**Terminal 1 - Simulation:** ```bash -# Terminal 1: Start Gazebo -ros2 launch gazebo/launch/ardupilot_drone.launch.py - -# Terminal 2: Start SITL -cd ~/ardupilot -sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map - -# Terminal 3: Run bridge + controllers -python run_ardupilot.py --no-sitl --pattern circular +source ~/ardu_ws/install/setup.bash +ros2 launch ardupilot_gz_bringup iris_runway.launch.py ``` -### Option 2: Manual Setup - +**Terminal 2 - Control:** ```bash -# Terminal 1: Start Gazebo world -gz sim -r gazebo/worlds/ardupilot_drone.sdf - -# Terminal 2: Start ArduPilot SITL -cd ~/ardupilot -sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console - -# Terminal 3: Run MAVLink bridge + controllers -python run_ardupilot.py --no-sitl +mavproxy.py --console --map --master=:14550 ``` -### Option 3: Full Automatic +## Installation ```bash -# Starts everything (requires SITL installed) -python run_ardupilot.py --pattern circular --console --map +./setup/install_ardupilot.sh +source ~/.bashrc ``` -## Flight Operations +This installs: +- ArduPilot SITL with DDS +- Gazebo with ardupilot_gz +- MAVProxy -### Using MAVProxy Commands - -Once connected, use MAVProxy to control the drone: +## MAVProxy Commands ```bash -# Set GUIDED mode for algorithm control +# Set mode mode guided -# Arm motors +# Arm arm throttle -# Take off to 5 meters +# Takeoff takeoff 5 # Land mode land - -# Disarm -disarm ``` -### Using the MAVLink Bridge API +## ROS 2 Topics -From Python, you can control the drone directly: +ArduPilot publishes native ROS 2 topics: -```python -from mavlink_bridge import MAVLinkBridge +```bash +# List topics +ros2 topic list -# Create bridge -bridge = MAVLinkBridge(sitl_port=14550) +# View position +ros2 topic echo /ap/geopose/filtered -# Arm and takeoff -bridge.set_mode('GUIDED') -bridge.arm() -bridge.takeoff(altitude=5.0) - -# Land -bridge.land() +# View battery +ros2 topic echo /ap/battery ``` -## Files +| Topic | Type | +|-------|------| +| `/ap/pose/filtered` | PoseStamped | +| `/ap/twist/filtered` | TwistStamped | +| `/ap/imu/filtered` | Imu | +| `/ap/battery` | BatteryState | -| File | Description | -|------|-------------| -| `mavlink_bridge.py` | ROS 2 ↔ MAVLink bridge | -| `run_ardupilot.py` | Integrated launcher | -| `gazebo/worlds/ardupilot_drone.sdf` | Gazebo world with ArduPilot plugin | -| `gazebo/launch/ardupilot_drone.launch.py` | ROS 2 launch file | +## Available Worlds -## Configuration +```bash +# Iris on runway +ros2 launch ardupilot_gz_bringup iris_runway.launch.py -Edit `config.py` to adjust ArduPilot settings: +# Iris in maze +ros2 launch ardupilot_gz_bringup iris_maze.launch.py -```python -ARDUPILOT = { - "vehicle": "ArduCopter", # ArduCopter, ArduPlane, APMrover2 - "frame": "gazebo-iris", # Gazebo frame - "sitl_host": "127.0.0.1", - "sitl_port": 5760, - "mavproxy_port": 14550, -} - -MAVLINK = { - "system_id": 1, - "component_id": 191, - "heartbeat_timeout": 5.0, -} +# Rover +ros2 launch ardupilot_gz_bringup wildthumper_playpen.launch.py ``` -## Telemetry Format +## Using the Launcher -The MAVLink bridge publishes telemetry in the same format as other modes: - -```json -{ - "imu": { - "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0} - }, - "altimeter": { - "altitude": 5.0, - "vertical_velocity": 0.0 - }, - "velocity": {"x": 0.0, "y": 0.0, "z": 0.0}, - "position": {"x": 0.0, "y": 0.0, "z": 5.0}, - "landing_pad": { - "relative_x": 0.5, - "relative_y": 0.2, - "distance": 4.8, - "confidence": 0.95 - }, - "battery": {"voltage": 12.6, "remaining": 100}, - "armed": true, - "flight_mode": "GUIDED", - "connected": true -} +```bash +python run_ardupilot.py --world runway +python run_ardupilot.py --world maze +python run_ardupilot.py --vehicle rover ``` ## Troubleshooting -### SITL Not Starting - +**No ROS 2 topics:** ```bash -# Check if SITL is installed -which sim_vehicle.py - -# Set ArduPilot path -export ARDUPILOT_HOME=~/ardupilot -export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest +# Check DDS is enabled +param set DDS_ENABLE 1 ``` -### Gazebo Plugin Not Found - +**Can't arm:** ```bash -# Check plugin path -echo $GZ_SIM_SYSTEM_PLUGIN_PATH - -# Verify plugin exists -ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so -``` - -### No MAVLink Connection - -```bash -# Check if SITL is listening -netstat -tuln | grep 14550 - -# Test with mavlink console -python -c "from pymavlink import mavutil; c = mavutil.mavlink_connection('udpin:127.0.0.1:14550'); print(c.wait_heartbeat())" -``` - -### Drone Won't Arm - -Common issues: -1. **Pre-arm checks failing** - Check MAVProxy console for errors -2. **GPS required** - In simulation, you may need to wait for GPS lock -3. **EKF not ready** - Wait for EKF to initialize - -Disable pre-arm checks for testing (not recommended for real flights): -``` -# In MAVProxy +# Disable pre-arm checks (simulation only) param set ARMING_CHECK 0 ``` - -## Flight Modes - -| Mode | Description | -|------|-------------| -| **GUIDED** | Accept velocity/position commands from controller | -| **LOITER** | Hold position (GPS required) | -| **ALT_HOLD** | Maintain altitude, manual horizontal | -| **LAND** | Automatic landing | -| **STABILIZE** | Attitude stabilization only | - -For autonomous landing, use **GUIDED** mode. - -## Architecture Comparison - -| Feature | Simple Gazebo | ArduPilot + Gazebo | -|---------|--------------|-------------------| -| Flight Controller | Velocity control | Full ArduPilot | -| Stabilization | Manual PD | Inbuilt EKF + PID | -| Flight Modes | None | All ArduPilot modes | -| Arming | Not required | Safety checks | -| Failsafes | None | Battery, GPS, etc. | -| MAVLink | No | Full protocol | -| GCS Support | No | QGC, Mission Planner | -| Realism | Low | High | diff --git a/docs/drone_guide.md b/docs/drone_guide.md index a707528..375b66d 100644 --- a/docs/drone_guide.md +++ b/docs/drone_guide.md @@ -1,28 +1,23 @@ -# DroneController Guide (GPS-Denied) +# DroneController Guide -Implement your landing algorithm in `drone_controller.py`. +Implement your GPS-denied landing algorithm in `drone_controller.py`. ## Quick Start 1. Edit `drone_controller.py` 2. Find `calculate_landing_maneuver()` 3. Implement your algorithm -4. Test with any mode: - - `python standalone_simulation.py --pattern stationary` (standalone) - - `python run_bridge.py --pattern stationary` (PyBullet + ROS 2) - - `python run_gazebo.py --pattern stationary` (Gazebo + ROS 2) +4. Test: `python standalone_simulation.py` -## GPS-Denied Challenge +## Sensors Available -No GPS available. You must use: - -| Sensor | Data | -|--------|------| -| **IMU** | Orientation, angular velocity | -| **Altimeter** | Altitude, vertical velocity | -| **Velocity** | Estimated from optical flow | -| **Camera** | 320x240 downward image (base64 JPEG) | -| **Landing Pad** | Relative position (may be null!) | +| Sensor | Description | +|--------|-------------| +| IMU | Orientation, angular velocity | +| Altimeter | Altitude, vertical velocity | +| Velocity | Estimated velocity (x, y, z) | +| Camera | 320x240 downward image | +| Landing Pad | Relative position (may be null!) | ## Function to Implement @@ -34,55 +29,20 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry): ## Sensor Data -### IMU ```python -imu = telemetry['imu'] -roll = imu['orientation']['roll'] -pitch = imu['orientation']['pitch'] -yaw = imu['orientation']['yaw'] -angular_vel = imu['angular_velocity'] # {x, y, z} -``` +# Altitude +altitude = telemetry['altimeter']['altitude'] +vertical_vel = telemetry['altimeter']['vertical_velocity'] -### Altimeter -```python -altimeter = telemetry['altimeter'] -altitude = altimeter['altitude'] -vertical_vel = altimeter['vertical_velocity'] -``` +# Velocity +vel_x = telemetry['velocity']['x'] +vel_y = telemetry['velocity']['y'] -### Velocity -```python -velocity = telemetry['velocity'] # {x, y, z} in m/s -``` - -### Camera -The drone has a downward-facing camera providing 320x240 JPEG images. - -```python -import base64 -from PIL import Image -import io - -camera = telemetry['camera'] -image_b64 = camera.get('image') - -if image_b64: - image_bytes = base64.b64decode(image_b64) - image = Image.open(io.BytesIO(image_bytes)) - # Process image for custom vision algorithms -``` - -### Landing Pad (Vision) -**Important: May be None if pad not visible!** - -```python -landing_pad = telemetry['landing_pad'] - -if landing_pad is not None: - relative_x = landing_pad['relative_x'] # body frame - relative_y = landing_pad['relative_y'] # body frame - distance = landing_pad['distance'] # vertical - confidence = landing_pad['confidence'] # 0-1 +# Landing Pad (may be None!) +landing_pad = telemetry.get('landing_pad') +if landing_pad: + relative_x = landing_pad['relative_x'] + relative_y = landing_pad['relative_y'] ``` ## Control Output @@ -112,7 +72,7 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry): thrust = 0.5 * (0 - altitude) - 0.3 * vertical_vel # Horizontal control - if landing_pad is not None: + if landing_pad: pitch = 0.3 * landing_pad['relative_x'] - 0.2 * vel_x roll = 0.3 * landing_pad['relative_y'] - 0.2 * vel_y else: @@ -124,84 +84,43 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry): ## Using the Camera -You can implement custom vision processing on the camera image: - ```python import cv2 import numpy as np import base64 -def process_camera(telemetry): - camera = telemetry.get('camera', {}) - image_b64 = camera.get('image') - - if not image_b64: - return None - - # Decode JPEG +camera = telemetry.get('camera', {}) +image_b64 = camera.get('image') + +if image_b64: image_bytes = base64.b64decode(image_b64) nparr = np.frombuffer(image_bytes, np.uint8) image = cv2.imdecode(nparr, cv2.IMREAD_COLOR) - - # Example: detect green landing pad - hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) - green_mask = cv2.inRange(hsv, (35, 50, 50), (85, 255, 255)) - - # Find contours - contours, _ = cv2.findContours(green_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - if contours: - largest = max(contours, key=cv2.contourArea) - M = cv2.moments(largest) - if M['m00'] > 0: - cx = int(M['m10'] / M['m00']) - cy = int(M['m01'] / M['m00']) - # cx, cy is center of detected pad in image coordinates - return (cx, cy) - - return None + # Process image... ``` -## Strategies - -### When Pad Not Visible -- Maintain altitude and stabilize -- Search by ascending or spiraling -- Dead reckoning from last known position - -### State Machine -1. Search → find pad -2. Approach → move above pad -3. Align → center over pad -4. Descend → controlled descent -5. Land → touch down - ## Testing ```bash -# Easy - stationary rover +# Easy - stationary python standalone_simulation.py --pattern stationary -# Medium - slow circular movement -python standalone_simulation.py --pattern circular --speed 0.2 +# Medium - circular +python standalone_simulation.py --pattern circular --speed 0.3 -# Hard - faster random movement -python standalone_simulation.py --pattern random --speed 0.3 - -# With ROS 2 (Gazebo) -ros2 launch gazebo/launch/drone_landing.launch.py # Terminal 1 -python run_gazebo.py --pattern circular # Terminal 2 +# Hard - random +python standalone_simulation.py --pattern random --speed 0.5 ``` ## Configuration -Edit `config.py` to tune controller gains: +Edit `config.py`: ```python CONTROLLER = { - "Kp_z": 0.5, # Altitude proportional gain - "Kd_z": 0.3, # Altitude derivative gain - "Kp_xy": 0.3, # Horizontal proportional gain - "Kd_xy": 0.2, # Horizontal derivative gain + "Kp_z": 0.5, # Altitude proportional + "Kd_z": 0.3, # Altitude derivative + "Kp_xy": 0.3, # Horizontal proportional + "Kd_xy": 0.2, # Horizontal derivative } ``` diff --git a/docs/gazebo.md b/docs/gazebo.md index 2f60580..64d82f9 100644 --- a/docs/gazebo.md +++ b/docs/gazebo.md @@ -1,159 +1,72 @@ # Gazebo Simulation Guide -Running the GPS-denied drone simulation with Gazebo Ignition Fortress on Linux/WSL2. +## Quick Start (2 Terminals) -## Quick Start (Two Terminals) - -**Terminal 1 - Launch Gazebo + Bridge:** +**Terminal 1 - Gazebo:** ```bash -source activate.sh ros2 launch gazebo/launch/drone_landing.launch.py ``` -**Terminal 2 - Run Controllers:** +**Terminal 2 - Controllers:** ```bash source activate.sh -python run_gazebo.py --pattern circular --speed 0.3 +python run_gazebo.py --pattern circular ``` -Both the drone AND rover will move! - ## Command Options ```bash -python run_gazebo.py --help +python run_gazebo.py --pattern circular --speed 0.5 Options: - --pattern stationary, linear, circular, square, random + --pattern, -p stationary, linear, circular, square, random --speed, -s Rover speed in m/s (default: 0.5) - --amplitude, -a Movement amplitude (default: 2.0) + --amplitude, -a Movement radius (default: 2.0) --no-rover Disable rover controller ``` -## How It Works - -1. **Gazebo** runs the physics simulation with: - - Drone with `VelocityControl` plugin (responds to `/drone/cmd_vel`) - - Rover with `VelocityControl` plugin (responds to `/rover/cmd_vel`) - -2. **ros_gz_bridge** connects Gazebo topics to ROS 2 - -3. **run_gazebo.py** starts: - - `GazeboBridge` - converts ROS topics to telemetry format - - `DroneController` - your landing algorithm - - `RoverController` - moves the landing pad - -## GPS-Denied Sensors - -The `GazeboBridge` provides the same sensor interface as PyBullet: - -| Sensor | Source | -|--------|--------| -| IMU | Gazebo odometry orientation | -| Altimeter | Gazebo Z position | -| Velocity | Gazebo twist | -| Camera | Gazebo camera sensor (if enabled) | -| Landing Pad | Computed from relative position | - -## Topics - -### ROS 2 Topics (your code uses these) - -| Topic | Type | Direction | -|-------|------|-----------| -| `/cmd_vel` | `Twist` | Input (from DroneController) | -| `/drone/telemetry` | `String` | Output (to DroneController) | -| `/rover/telemetry` | `String` | Output (rover position) | - -### Gazebo Topics (bridged automatically) - -| Topic | Type | Description | -|-------|------|-------------| -| `/drone/cmd_vel` | `Twist` | Drone velocity commands | -| `/rover/cmd_vel` | `Twist` | Rover velocity commands | -| `/model/drone/odometry` | `Odometry` | Drone state | -| `/drone/imu` | `IMU` | IMU sensor data | -| `/clock` | `Clock` | Simulation time | - -## Headless Mode (WSL2 / No GPU) - -Run Gazebo without GUI: +## View Camera ```bash -# Server mode only +python camera_viewer.py --topic /drone/camera +``` + +## Sensors + +| Sensor | Description | +|--------|-------------| +| IMU | Orientation, angular velocity | +| Altimeter | Altitude, vertical velocity | +| Velocity | Estimated velocity (x, y, z) | +| Camera | Downward-facing image | +| Landing Pad | Relative position | + +## ROS 2 Topics + +| Topic | Direction | +|-------|-----------| +| `/cmd_vel` | Your commands → Drone | +| `/drone/telemetry` | Sensors → You | +| `/drone/camera` | Camera → You | +| `/rover/telemetry` | Rover position | + +## Headless Mode (WSL2) + +```bash +# Server only (no GUI) ign gazebo -s gazebo/worlds/drone_landing.sdf ``` -Then run the bridge manually: -```bash -ros2 run ros_gz_bridge parameter_bridge \ - /drone/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist \ - /rover/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist \ - /model/drone/odometry@nav_msgs/msg/Odometry[ignition.msgs.Odometry -``` - -## World File Details - -The world file `gazebo/worlds/drone_landing.sdf` includes: - -- **Drone** at (0, 0, 2) with: - - `VelocityControl` plugin for movement - - `OdometryPublisher` plugin for telemetry - - IMU sensor - -- **Landing Pad (Rover)** at (0, 0, 0.15) with: - - `VelocityControl` plugin for movement - - Visual H marker - ## Troubleshooting -### Drone falls immediately +**Drone falls:** +- Check `run_gazebo.py` is running +- Check topic: `ros2 topic echo /drone/cmd_vel` -The drone should hover with the controller running. If it falls: -1. Check that `run_gazebo.py` is running -2. Verify the bridge shows "Passing message from ROS" -3. Check `/drone/cmd_vel` topic: `ros2 topic echo /drone/cmd_vel` +**Rover doesn't move:** +- Check topic: `ros2 topic echo /rover/cmd_vel` -### Rover doesn't move - -1. Check that `/rover/cmd_vel` is bridged -2. Verify RoverController is publishing: `ros2 topic echo /rover/cmd_vel` - -### Model not found - -Set the model path: +**Model not found:** ```bash export GZ_SIM_RESOURCE_PATH=$PWD/gazebo/models:$GZ_SIM_RESOURCE_PATH -export IGN_GAZEBO_RESOURCE_PATH=$PWD/gazebo/models:$IGN_GAZEBO_RESOURCE_PATH ``` - -### "Cannot connect to display" (WSL2) - -Use headless mode: -```bash -ign gazebo -s gazebo/worlds/drone_landing.sdf -``` - -Or ensure WSLg is working: -```bash -export DISPLAY=:0 -``` - -### Plugin not found - -For Ignition Fortress, plugins use `libignition-gazebo-*-system.so` naming. - -Check available plugins: -```bash -ls /usr/lib/x86_64-linux-gnu/ign-gazebo-6/plugins/ -``` - -## Launch File Options - -```bash -ros2 launch gazebo/launch/drone_landing.launch.py use_sim_time:=true -``` - -| Argument | Default | Description | -|----------|---------|-------------| -| `use_sim_time` | `true` | Use Gazebo clock | diff --git a/docs/installation.md b/docs/installation.md index ae96b6b..d7d4c4e 100644 --- a/docs/installation.md +++ b/docs/installation.md @@ -1,503 +1,125 @@ # Installation Guide -Setup instructions for all supported platforms. - ## Quick Install +```bash +# Ubuntu/Debian +./setup/install_ubuntu.sh +source activate.sh + +# Test +python standalone_simulation.py +``` + +## Install Scripts + | Platform | Command | |----------|---------| | Ubuntu/Debian | `./setup/install_ubuntu.sh` | -| Ubuntu + ArduPilot | `./setup/install_ubuntu.sh --with-ardupilot` | +| ArduPilot SITL | `./setup/install_ardupilot.sh` | | Arch Linux | `./setup/install_arch.sh` | | macOS | `./setup/install_macos.sh` | | Windows | `.\setup\install_windows.ps1` | -After installation: -```bash -source activate.sh # Linux/macOS -. .\activate.ps1 # Windows PowerShell +## Platform Support -python standalone_simulation.py -``` +| Mode | Ubuntu | macOS | Windows | +|------|--------|-------|---------| +| Standalone | ✅ | ✅ | ✅ | +| Gazebo + ROS 2 | ✅ | ❌ | WSL2 | +| ArduPilot SITL | ✅ | ❌ | WSL2 | --- -## Platform Compatibility - -| Feature | Ubuntu | Arch | macOS | Windows | WSL2 | -|---------|--------|------|-------|---------|------| -| **Standalone Simulation** | ✅ | ✅ | ✅ | ✅ | ✅ | -| **ROS 2** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ | -| **Gazebo** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ | -| **ArduPilot SITL** | ✅ | ⚠️ Manual | ❌ | ❌ | ✅ | -| **Full Mode** | ✅ | ⚠️ | ❌ | ❌ | ✅ | -| **GUI Support** | ✅ | ✅ | ✅ | ✅ | ✅ WSLg | - -**Legend:** -- ✅ Fully supported -- ⚠️ Available but requires extra setup -- ❌ Not supported - -**Recommendation for Windows users:** Use WSL2 for the full experience (ROS 2 + Gazebo). - ---- - -## Ubuntu / Debian - -**Tested on:** Ubuntu 22.04 (Jammy), Ubuntu 24.04 (Noble) +## Ubuntu/Debian ```bash -# Run installer ./setup/install_ubuntu.sh - -# Activate environment source activate.sh - -# Run simulation python standalone_simulation.py ``` **Installs:** -- ROS 2 (Humble or Jazzy based on Ubuntu version) -- Gazebo (ros-gz) -- Python packages: pybullet, numpy, pillow, pyinstaller, pymavlink +- ROS 2 Humble/Jazzy +- Gazebo +- Python packages (pybullet, numpy, opencv, pymavlink) + +--- + +## ArduPilot SITL + +For realistic flight controller simulation: -**With ArduPilot SITL (full flight controller):** ```bash -# Run installer with ArduPilot -./setup/install_ubuntu.sh --with-ardupilot +./setup/install_ardupilot.sh +source ~/.bashrc +``` -# This will also install: -# - ArduPilot SITL (~15-20 min build) -# - ArduPilot Gazebo plugin -# - MAVProxy +**Installs:** +- ArduPilot SITL +- ardupilot_gz (Gazebo integration) +- MAVProxy + +**Run:** +```bash +# Terminal 1 +source ~/ardu_ws/install/setup.bash +ros2 launch ardupilot_gz_bringup iris_runway.launch.py + +# Terminal 2 +mavproxy.py --console --map --master=:14550 ``` --- -## Arch Linux +## Windows (WSL2) -**Tested on:** Arch Linux (rolling release) - -```bash -# Run installer -./setup/install_arch.sh - -# Activate environment -source activate.sh - -# Run simulation -python standalone_simulation.py +1. Install WSL2: +```powershell +wsl --install -d Ubuntu-22.04 ``` -**Installs:** -- Python packages: pybullet, numpy, pillow, pyinstaller -- yay (AUR helper) - -**Optional ROS 2 (from AUR):** +2. Open Ubuntu and run: ```bash -yay -S ros-humble-desktop -yay -S ros-humble-ros-gz +./setup/install_ubuntu.sh +source activate.sh +python standalone_simulation.py ``` --- ## macOS -**Tested on:** macOS 12+ (Monterey, Ventura, Sonoma) - ```bash -# Run installer ./setup/install_macos.sh - -# Activate environment source activate.sh - -# Run simulation python standalone_simulation.py ``` -**Installs:** -- Homebrew (if not present) -- Python 3.11 -- Python packages: pybullet, numpy, pillow, pyinstaller - -**Note:** ROS 2 and Gazebo are not supported on macOS. Use standalone mode. +**Note:** ROS 2 and Gazebo not supported on macOS. Use standalone mode. --- -## Windows - -**Tested on:** Windows 10, Windows 11 - -```powershell -# Open PowerShell as Administrator -Set-ExecutionPolicy RemoteSigned -Scope CurrentUser - -# Run installer -.\setup\install_windows.ps1 - -# Activate environment -. .\activate.ps1 - -# Run simulation -python standalone_simulation.py -``` - -**Installs:** -- Chocolatey (package manager) -- Python 3.11 -- Python packages: pybullet, numpy, pillow, pyinstaller - -**Note:** ROS 2 and Gazebo are not supported natively on Windows. Use standalone mode or WSL2 (below). - ---- - -## Windows with WSL2 (Full Linux Experience) - -WSL2 lets you run full Linux on Windows with GUI support. This enables ROS 2 and Gazebo! - -**Requirements:** Windows 10 (build 19041+) or Windows 11 - -### Step 1: Install WSL2 - -Open PowerShell as Administrator: - -```powershell -# Install WSL2 with Ubuntu -wsl --install -d Ubuntu-22.04 - -# set the user name and password -# Then update the system -sudo apt update -sudo apt upgrade - -# Restart computer if prompted -``` - -### Step 2: Enable GUI Support (WSLg) - -Windows 11 and recent Windows 10 updates include WSLg (GUI support) automatically. - -Verify by opening Ubuntu and running: -```bash -# Test GUI (should open a window) -sudo apt update -sudo apt install x11-apps -y -xclock -``` - -If xclock appears, GUI is working! - -### Step 3: Install Simulation in WSL - -Open Ubuntu from Start menu: - -```bash -# Clone or copy your project -cd ~ -git clone simulation -# OR copy from Windows: -# cp -r /mnt/c/Users/YourName/simulation ~/simulation - -cd simulation - -# Run Ubuntu installer -./setup/install_ubuntu.sh - -# Activate -source activate.sh - -# Run with GUI -python standalone_simulation.py -``` - -### WSL2 Tips - -**Access Windows files:** -```bash -# Windows C: drive is at /mnt/c/ -cd /mnt/c/Users/YourName/Documents -``` - -**Run from Windows Terminal:** -```powershell -wsl -d Ubuntu-22.04 -e bash -c "cd ~/simulation && source activate.sh && python standalone_simulation.py" -``` - -**GPU Acceleration (NVIDIA):** - -If you have an NVIDIA GPU: -```bash -# Check if GPU is available -nvidia-smi - -# PyBullet will use hardware rendering automatically -``` - -**Install Gazebo (optional):** - -If you want to use Gazebo simulation: -```bash -# Install ros-gz bridge -sudo apt install ros-humble-ros-gz - -# Install Gazebo Fortress (provides 'ign' command) -sudo apt install gz-fortress - -# Verify - one of these should work: -gz sim --version # Newer Gazebo -ign gazebo --version # Fortress (ROS 2 Humble) -``` - -**Note:** ROS 2 Humble uses Gazebo Fortress, which uses `ign gazebo` command instead of `gz sim`. The launch file auto-detects which command is available. - -**Gazebo GPU Issues in WSL2:** - -If Gazebo crashes with GPU/OpenGL errors, try: - -```bash -# Option 1: Run in server mode (no GUI) -ign gazebo -s gazebo/worlds/drone_landing.sdf - -# Option 2: Fix permissions and restart WSL -sudo usermod -aG render $USER -chmod 700 /run/user/1000 -# Then in PowerShell: wsl --shutdown - -# Option 3: Force software rendering -export LIBGL_ALWAYS_SOFTWARE=1 -ign gazebo gazebo/worlds/drone_landing.sdf - -# Option 4: Just use PyBullet (more reliable on WSL2) -python standalone_simulation.py -``` - -**Troubleshooting WSL GUI:** - -If GUI doesn't work: -```bash -# Update WSL -wsl --update - -# Set WSL2 as default -wsl --set-default-version 2 - -# Reinstall Ubuntu -wsl --unregister Ubuntu-22.04 -wsl --install -d Ubuntu-22.04 -``` - -**Using VcXsrv (older Windows 10):** - -If WSLg isn't available: -```powershell -# Install VcXsrv -choco install vcxsrv -y -``` - -Then in WSL: -```bash -# Add to ~/.bashrc -export DISPLAY=$(grep -m 1 nameserver /etc/resolv.conf | awk '{print $2}'):0 -export LIBGL_ALWAYS_INDIRECT=1 - -# Start VcXsrv with "Disable access control" checked -# Then run simulation -python standalone_simulation.py -``` - ---- - -## Manual Installation - -If the install scripts don't work, install manually: - -### 1. Python 3.10+ - -```bash -# Ubuntu/Debian -sudo apt install python3 python3-pip python3-venv - -# Arch -sudo pacman -S python python-pip python-virtualenv - -# macOS -brew install python@3.11 - -# Windows -# Download from https://python.org -``` - -### 2. Create Virtual Environment +## Manual Install ```bash +# Create virtual environment python3 -m venv venv -source venv/bin/activate # Linux/macOS -# OR -.\venv\Scripts\Activate.ps1 # Windows -``` +source venv/bin/activate -### 3. Install Python Packages - -```bash +# Install packages pip install -r requirements.txt -``` -Or manually: -```bash -pip install pybullet numpy pillow pyinstaller -``` - -### 4. Run Simulation - -```bash +# Run python standalone_simulation.py ``` --- -## Troubleshooting - -### PyBullet fails to install - -Install build tools: - -```bash -# Ubuntu/Debian -sudo apt install build-essential - -# Arch -sudo pacman -S base-devel - -# macOS -xcode-select --install - -# Windows -# Install Visual Studio Build Tools -``` - -### "Cannot connect to X server" - -PyBullet GUI requires a display: - -```bash -# Use virtual display -sudo apt install xvfb -xvfb-run python standalone_simulation.py - -# OR use X11 forwarding -ssh -X user@host -``` - -### Pillow fails to install - -```bash -# Ubuntu/Debian -sudo apt install libjpeg-dev zlib1g-dev - -# Arch -sudo pacman -S libjpeg-turbo zlib - -# macOS -brew install libjpeg zlib -``` - -### Permission denied on Windows - -Run PowerShell as Administrator: -```powershell -Set-ExecutionPolicy RemoteSigned -Scope CurrentUser -``` - ---- - ## Verification -After installation, verify packages: - ```bash python -c "import pybullet; print('PyBullet OK')" -python -c "import numpy; print('NumPy OK')" -python -c "from PIL import Image; print('Pillow OK')" +python -c "import cv2; print('OpenCV OK')" python -c "from pymavlink import mavutil; print('pymavlink OK')" ``` - -All should print "OK". - ---- - -## ArduPilot SITL Manual Setup - -If you want to install ArduPilot SITL manually (without the install script): - -### 1. Install ArduPilot - -```bash -# Clone ArduPilot -git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git ~/ardupilot -cd ~/ardupilot - -# Install prerequisites (Ubuntu) -Tools/environment_install/install-prereqs-ubuntu.sh -y - -# Reload profile -. ~/.profile - -# Build ArduCopter SITL -./waf configure --board sitl -./waf copter -``` - -### 2. Install ArduPilot Gazebo Plugin - -```bash -# Clone plugin -git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo -cd ~/ardupilot_gazebo - -# Build -mkdir build && cd build -cmake .. -DCMAKE_BUILD_TYPE=Release -make -j$(nproc) -``` - -### 3. Set Environment Variables - -Add to `~/.bashrc`: - -```bash -# ArduPilot -export ARDUPILOT_HOME=$HOME/ardupilot -export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest - -# ArduPilot Gazebo Plugin -export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH -export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH -``` - -### 4. Test SITL - -```bash -# Test ArduCopter SITL -cd ~/ardupilot -sim_vehicle.py -v ArduCopter --console --map -``` - -### 5. Run with Gazebo - -```bash -# Terminal 1: Launch Gazebo -ros2 launch gazebo/launch/ardupilot_drone.launch.py - -# Terminal 2: Start SITL -cd ~/ardupilot -sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console - -# Terminal 3: Run controllers -cd ~/simulation -source activate.sh -python run_ardupilot.py --no-sitl --pattern circular -``` - -For more details, see [ArduPilot Guide](ardupilot.md). diff --git a/docs/protocol.md b/docs/protocol.md index e2fb433..7d1c53b 100644 --- a/docs/protocol.md +++ b/docs/protocol.md @@ -1,8 +1,8 @@ -# Communication Protocol (GPS-Denied) +# Communication Protocol -Message formats for GPS-denied drone operation with camera. +Message formats for drone operation. -## Drone Commands +## Commands ```json { @@ -13,25 +13,20 @@ Message formats for GPS-denied drone operation with camera. } ``` -| Field | Range | Description | -|-------|-------|-------------| -| `thrust` | ±1.0 | Vertical thrust (positive = up) | -| `pitch` | ±0.5 | Forward/backward tilt | -| `roll` | ±0.5 | Left/right tilt | -| `yaw` | ±0.5 | Rotation | +| Field | Range | Effect | +|-------|-------|--------| +| thrust | ±1.0 | Up/down | +| pitch | ±0.5 | Forward/back | +| roll | ±0.5 | Left/right | +| yaw | ±0.5 | Rotation | ---- - -## Drone Telemetry - -Published on `/drone/telemetry`. **No GPS position available.** +## Telemetry ```json { "imu": { "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}, - "linear_acceleration": {"x": 0.0, "y": 0.0, "z": 9.81} + "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0} }, "altimeter": { "altitude": 5.0, @@ -47,138 +42,30 @@ Published on `/drone/telemetry`. **No GPS position available.** "camera": { "width": 320, "height": 240, - "fov": 60.0, - "image": "" - }, - "landed": false, - "timestamp": 1.234 + "image": "" + } } ``` ---- +## Sensors -## Sensor Details +| Sensor | Fields | +|--------|--------| +| IMU | orientation (roll, pitch, yaw), angular_velocity | +| Altimeter | altitude, vertical_velocity | +| Velocity | x, y, z (m/s) | +| Landing Pad | relative_x, relative_y, distance, confidence | +| Camera | Base64 JPEG image | -### IMU -Always available. - -| Field | Unit | Description | -|-------|------|-------------| -| `orientation.roll/pitch/yaw` | radians | Euler angles | -| `angular_velocity.x/y/z` | rad/s | Rotation rates | -| `linear_acceleration.x/y/z` | m/s² | Acceleration | - -### Altimeter -Always available. - -| Field | Unit | Description | -|-------|------|-------------| -| `altitude` | meters | Height above ground | -| `vertical_velocity` | m/s | Vertical speed | - -### Velocity -Estimated from optical flow. - -| Field | Unit | Description | -|-------|------|-------------| -| `x` | m/s | Forward velocity | -| `y` | m/s | Lateral velocity | -| `z` | m/s | Vertical velocity | - -### Landing Pad Detection -**May be null if pad not visible!** - -| Field | Unit | Description | -|-------|------|-------------| -| `relative_x` | meters | Forward/back offset (body frame) | -| `relative_y` | meters | Left/right offset (body frame) | -| `distance` | meters | Vertical distance to pad | -| `confidence` | 0-1 | Detection confidence | - -### Camera -Always available. - -| Field | Description | -|-------|-------------| -| `width` | Image width in pixels | -| `height` | Image height in pixels | -| `fov` | Horizontal field of view in degrees | -| `image` | Base64 encoded JPEG (or null) | - ---- - -## Using the Camera Image - -The camera provides a base64-encoded JPEG image of what the drone sees looking down. - -### Decoding the Image (Python) - -```python -import base64 -from PIL import Image -import io - -def decode_camera_image(telemetry): - camera = telemetry.get('camera', {}) - image_b64 = camera.get('image') - - if image_b64 is None: - return None - - # Decode base64 to bytes - image_bytes = base64.b64decode(image_b64) - - # Load as PIL Image - image = Image.open(io.BytesIO(image_bytes)) - - return image -``` - -### Using with OpenCV +## Decoding Camera ```python import base64 import cv2 import numpy as np -def decode_camera_image_cv2(telemetry): - camera = telemetry.get('camera', {}) - image_b64 = camera.get('image') - - if image_b64 is None: - return None - - # Decode base64 to bytes - image_bytes = base64.b64decode(image_b64) - - # Convert to numpy array - nparr = np.frombuffer(image_bytes, np.uint8) - - # Decode JPEG - image = cv2.imdecode(nparr, cv2.IMREAD_COLOR) - - return image -``` - -### Image Properties - -- **Resolution**: 320 x 240 pixels -- **Format**: JPEG (quality 70) -- **FOV**: 60 degrees -- **Direction**: Downward-facing -- **Update Rate**: ~5 Hz (every 5th telemetry frame) - ---- - -## Rover Telemetry - -For internal use by RoverController. - -```json -{ - "position": {"x": 1.5, "y": 0.8, "z": 0.15}, - "velocity": {"x": 0.3, "y": 0.4, "z": 0.0}, - "pattern": "circular", - "timestamp": 1.234 -} +image_b64 = telemetry['camera']['image'] +image_bytes = base64.b64decode(image_b64) +nparr = np.frombuffer(image_bytes, np.uint8) +image = cv2.imdecode(nparr, cv2.IMREAD_COLOR) ``` diff --git a/docs/pybullet.md b/docs/pybullet.md index e677010..a63b5ba 100644 --- a/docs/pybullet.md +++ b/docs/pybullet.md @@ -1,163 +1,68 @@ # PyBullet Simulation Guide -Running the GPS-denied drone simulation with PyBullet physics engine. +## Standalone Mode (1 Terminal) -## Standalone Mode (Single Terminal - Any Platform) - -No ROS 2 required! Works on Windows, macOS, and Linux: +No ROS 2 required. Works on Windows, macOS, Linux: ```bash -source activate.sh # Linux/macOS -. .\activate.ps1 # Windows - -python standalone_simulation.py --pattern circular --speed 0.3 +source activate.sh +python standalone_simulation.py --pattern circular ``` -### Options - -```bash -python standalone_simulation.py --help - -Options: - --pattern, -p stationary, linear, circular, square - --speed, -s Rover speed in m/s (default: 0.5) - --amplitude, -a Movement amplitude in meters (default: 2.0) -``` - ---- - -## ROS 2 Mode (Two Terminals) - -For distributed or remote simulation with ROS 2: +## ROS 2 Mode (2 Terminals) **Terminal 1 - Simulator:** ```bash -source activate.sh python simulation_host.py ``` **Terminal 2 - Controllers:** ```bash -source activate.sh -python run_bridge.py --pattern circular --speed 0.3 +python run_bridge.py --pattern circular ``` -### How It Works +## Options -1. `simulation_host.py` runs PyBullet physics and listens on UDP port 5555 -2. `run_bridge.py` starts: - - `ROS2SimulatorBridge` - connects ROS topics to UDP - - `DroneController` - your landing algorithm - - `RoverController` - moves the landing pad - -The rover position is sent to the simulator, so both drone AND rover move! - -### Remote Setup - -Run simulator on one machine, controllers on another: - -**Machine 1 (with display):** ```bash -python simulation_host.py # Listens on 0.0.0.0:5555 +--pattern, -p stationary, linear, circular, square, random +--speed, -s Rover speed in m/s (default: 0.5) +--amplitude, -a Movement radius (default: 2.0) ``` -**Machine 2 (headless):** -```bash -python run_bridge.py --host 192.168.1.100 --pattern circular -``` +## Remote Setup ---- +**Machine 1:** `python simulation_host.py` +**Machine 2:** `python run_bridge.py --host ` -## Configuration - -All parameters are configurable in `config.py`: - -```python -DRONE = { - "mass": 1.0, - "start_position": (0.0, 0.0, 5.0), - "thrust_scale": 15.0, - ... -} - -ROVER = { - "start_position": (0.0, 0.0, 0.15), - "default_pattern": "circular", - "default_speed": 0.5, - ... -} - -CONTROLLER = { - "Kp_z": 0.5, - "Kd_z": 0.3, - ... -} -``` - ---- - -## Simulation Parameters - -| Parameter | Value | -|-----------|-------| -| Physics Rate | 240 Hz | -| Telemetry Rate | 24 Hz | -| Drone Mass | 1.0 kg (configurable) | -| Rover Mass | Static (kinematic) | -| UDP Port | 5555 (commands), 5556 (telemetry) | - -## GPS-Denied Sensors +## Sensors | Sensor | Description | |--------|-------------| -| **IMU** | Orientation (roll, pitch, yaw), angular velocity | -| **Altimeter** | Altitude above ground, vertical velocity | -| **Velocity** | Estimated horizontal velocity (x, y, z) | -| **Camera** | 320x240 downward-facing JPEG image | -| **Landing Pad** | Vision-based relative position when in camera FOV | +| IMU | Orientation, angular velocity | +| Altimeter | Altitude, vertical velocity | +| Velocity | Estimated velocity (x, y, z) | +| Camera | 320x240 downward JPEG | +| Landing Pad | Relative position | -## Troubleshooting +## Configuration -### "Cannot connect to X server" +Edit `config.py`: -PyBullet GUI requires a display: -```bash -# Use virtual display -xvfb-run python standalone_simulation.py - -# Or use X11 forwarding -ssh -X user@host -``` - -### Drone flies erratically - -Reduce control gains in `config.py`: ```python CONTROLLER = { - "Kp_z": 0.3, - "Kd_z": 0.2, - "Kp_xy": 0.2, - "Kd_xy": 0.1, + "Kp_z": 0.5, + "Kd_z": 0.3, + "Kp_xy": 0.3, + "Kd_xy": 0.2, } ``` -### Camera image not appearing +## Troubleshooting -Install Pillow: +**"Cannot connect to X server":** ```bash -pip install pillow numpy +xvfb-run python standalone_simulation.py ``` -### Rover not moving (ROS 2 mode) - -Ensure `run_bridge.py` is used (not `ros_bridge.py` directly). -The rover controller must be running to send position updates. - -### WSL2 GUI issues - -Set display scaling: -```bash -export GDK_DPI_SCALE=1.0 -export QT_SCALE_FACTOR=1.0 -python standalone_simulation.py -``` +**Drone flies erratically:** +Reduce gains in `config.py` diff --git a/docs/rover_controller.md b/docs/rover_controller.md index 914cff4..a14c292 100644 --- a/docs/rover_controller.md +++ b/docs/rover_controller.md @@ -4,97 +4,58 @@ The RoverController creates a moving landing pad target. ## Usage -The rover controller is automatically included when running `controllers.py`: - ```bash -# Stationary rover (default) -python controllers.py +# Stationary (default) +python standalone_simulation.py --pattern stationary -# Moving rover -python controllers.py --pattern circular --speed 0.3 +# Moving +python standalone_simulation.py --pattern circular --speed 0.3 ``` -### Options +## Options -| Option | Short | Default | Description | -|--------|-------|---------|-------------| -| `--pattern` | `-p` | stationary | Movement pattern | -| `--speed` | `-s` | 0.5 | Speed in m/s | -| `--amplitude` | `-a` | 2.0 | Amplitude in meters | +| Option | Default | Description | +|--------|---------|-------------| +| `--pattern, -p` | stationary | Movement pattern | +| `--speed, -s` | 0.5 | Speed in m/s | +| `--amplitude, -a` | 2.0 | Radius in meters | -## Movement Patterns +## Patterns -### Stationary -```bash -python controllers.py --pattern stationary -``` -Rover stays at origin. Best for initial testing. - -### Linear -```bash -python controllers.py --pattern linear --speed 0.3 --amplitude 2.0 -``` -Oscillates along X-axis. - -### Circular -```bash -python controllers.py --pattern circular --speed 0.5 --amplitude 2.0 -``` -Follows circular path of radius `amplitude`. - -### Random -```bash -python controllers.py --pattern random --speed 0.3 --amplitude 2.0 -``` -Moves to random positions. Changes target every 3 seconds. - -### Square -```bash -python controllers.py --pattern square --speed 0.5 --amplitude 2.0 -``` -Square pattern with corners at `(±amplitude, ±amplitude)`. +| Pattern | Description | +|---------|-------------| +| stationary | Stays at origin | +| linear | Oscillates along X-axis | +| circular | Circular path | +| square | Square with sharp turns | +| random | Random positions | ## Difficulty Levels -| Level | Pattern | Speed | Description | -|-------|---------|-------|-------------| -| Beginner | stationary | 0.0 | Static target | -| Easy | linear | 0.2 | Predictable 1D | -| Medium | circular | 0.3 | Smooth 2D | -| Hard | random | 0.3 | Unpredictable | -| Expert | square | 0.5 | Sharp turns | +| Level | Pattern | Speed | +|-------|---------|-------| +| Beginner | stationary | 0.0 | +| Easy | linear | 0.2 | +| Medium | circular | 0.3 | +| Hard | random | 0.3 | +| Expert | square | 0.5 | ## Progressive Testing -Start easy and increase difficulty: - ```bash -# Step 1: Static target -python controllers.py --pattern stationary +# 1. Static target +python standalone_simulation.py --pattern stationary -# Step 2: Slow linear motion -python controllers.py --pattern linear --speed 0.2 +# 2. Slow circular +python standalone_simulation.py --pattern circular --speed 0.2 -# Step 3: Slow circular motion -python controllers.py --pattern circular --speed 0.2 +# 3. Faster circular +python standalone_simulation.py --pattern circular --speed 0.4 -# Step 4: Faster circular -python controllers.py --pattern circular --speed 0.4 - -# Step 5: Random -python controllers.py --pattern random --speed 0.3 +# 4. Random +python standalone_simulation.py --pattern random --speed 0.3 ``` -## Published Topics +## Note -| Topic | Type | Description | -|-------|------|-------------| -| `/rover/cmd_vel` | `Twist` | Velocity commands | -| `/rover/position` | `Point` | Current position | -| `/rover/telemetry` | `String` | Full state (JSON) | - -## GPS-Denied Note - -In GPS-denied mode, the drone cannot directly access rover position. Instead, it must detect the landing pad visually via `landing_pad` sensor data. - -The `/rover/telemetry` topic is used internally by the RoverController but the DroneController should primarily rely on vision-based `landing_pad` detection in the drone telemetry. +The drone cannot access rover position directly (GPS-denied). It must detect the landing pad visually via the camera. diff --git a/drone_controller.py b/drone_controller.py index 5be2f4e..1b218ac 100644 --- a/drone_controller.py +++ b/drone_controller.py @@ -1,16 +1,24 @@ #!/usr/bin/env python3 """ DroneController - Template for GPS-denied landing logic. + +Supports multiple telemetry sources: + 1. Custom JSON telemetry (/drone/telemetry) - PyBullet/Gazebo bridge + 2. ArduPilot ROS 2 DDS topics (/ap/*) - Official ArduPilot integration + Implement your algorithm in calculate_landing_maneuver(). Uses sensors: IMU, altimeter, camera, and landing pad detection. """ import json +import math from typing import Dict, Any, Optional import rclpy from rclpy.node import Node -from geometry_msgs.msg import Twist +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +from geometry_msgs.msg import Twist, PoseStamped, TwistStamped +from sensor_msgs.msg import Imu, BatteryState, NavSatFix from std_msgs.msg import String # Load configuration @@ -24,10 +32,38 @@ except ImportError: LANDING = {"height_threshold": 0.1, "success_velocity": 0.1} -class DroneController(Node): - """Drone controller for GPS-denied landing.""" +def quaternion_to_euler(x: float, y: float, z: float, w: float) -> tuple: + """Convert quaternion to Euler angles (roll, pitch, yaw).""" + # Roll (x-axis rotation) + sinr_cosp = 2.0 * (w * x + y * z) + cosr_cosp = 1.0 - 2.0 * (x * x + y * y) + roll = math.atan2(sinr_cosp, cosr_cosp) + + # 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 - def __init__(self): + +class DroneController(Node): + """ + Drone controller for GPS-denied landing. + + Supports multiple telemetry sources: + - Legacy: /drone/telemetry (JSON String) + - ArduPilot DDS: /ap/pose/filtered, /ap/imu/filtered, etc. + """ + + def __init__(self, use_ardupilot_topics: bool = True): super().__init__('drone_controller') # Load from config @@ -42,37 +78,189 @@ class DroneController(Node): self._landing_height = LANDING.get("height_threshold", 0.1) self._landing_velocity = LANDING.get("success_velocity", 0.1) + # Mode selection + self._use_ardupilot = use_ardupilot_topics + self.get_logger().info('=' * 50) self.get_logger().info('Drone Controller Starting (GPS-Denied)...') if CONFIG_LOADED: self.get_logger().info(' Configuration loaded from config.py') + self.get_logger().info(f' Mode: {"ArduPilot DDS" if use_ardupilot_topics else "Legacy JSON"}') self.get_logger().info('=' * 50) + # State variables self._latest_telemetry: Optional[Dict[str, Any]] = None self._rover_telemetry: Optional[Dict[str, Any]] = None self._telemetry_received = False self._landing_complete = False - self._telemetry_sub = self.create_subscription( - String, '/drone/telemetry', self._telemetry_callback, 10 - ) - self.get_logger().info(' Subscribed to: /drone/telemetry') + # ArduPilot state (built from DDS topics) + self._ap_pose: Optional[PoseStamped] = None + self._ap_twist: Optional[TwistStamped] = None + self._ap_imu: Optional[Imu] = None + self._ap_battery: Optional[BatteryState] = None + # QoS for sensor topics + sensor_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1 + ) + + if use_ardupilot_topics: + self._setup_ardupilot_subscriptions(sensor_qos) + else: + self._setup_legacy_subscriptions() + + # Always subscribe to rover telemetry (for moving landing pad) self._rover_telemetry_sub = self.create_subscription( String, '/rover/telemetry', self._rover_telemetry_callback, 10 ) self.get_logger().info(' Subscribed to: /rover/telemetry') + # Command publisher self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10) self.get_logger().info(' Publishing to: /cmd_vel') + # Control loop control_period = 1.0 / self._control_rate self._control_timer = self.create_timer(control_period, self._control_loop) self.get_logger().info(f' Control loop: {self._control_rate} Hz') self.get_logger().info('Drone Controller Ready!') - self.get_logger().info('Sensors: IMU, Altimeter, Camera, Landing Pad Detection') - + self.get_logger().info('Sensors: IMU, Altimeter, Velocity, Landing Pad Detection') + + def _setup_ardupilot_subscriptions(self, qos: QoSProfile): + """Set up subscriptions to official ArduPilot ROS 2 topics.""" + + # Pose (position + orientation) + self._ap_pose_sub = self.create_subscription( + PoseStamped, '/ap/pose/filtered', self._ap_pose_callback, qos + ) + self.get_logger().info(' Subscribed to: /ap/pose/filtered') + + # Twist (velocity) + self._ap_twist_sub = self.create_subscription( + TwistStamped, '/ap/twist/filtered', self._ap_twist_callback, qos + ) + self.get_logger().info(' Subscribed to: /ap/twist/filtered') + + # IMU + self._ap_imu_sub = self.create_subscription( + Imu, '/ap/imu/filtered', self._ap_imu_callback, qos + ) + self.get_logger().info(' Subscribed to: /ap/imu/filtered') + + # Battery + self._ap_battery_sub = self.create_subscription( + BatteryState, '/ap/battery', self._ap_battery_callback, qos + ) + self.get_logger().info(' Subscribed to: /ap/battery') + + def _setup_legacy_subscriptions(self): + """Set up subscriptions to legacy JSON telemetry.""" + self._telemetry_sub = self.create_subscription( + String, '/drone/telemetry', self._telemetry_callback, 10 + ) + self.get_logger().info(' Subscribed to: /drone/telemetry') + + # ArduPilot topic callbacks + def _ap_pose_callback(self, msg: PoseStamped): + self._ap_pose = msg + if not self._telemetry_received: + self._telemetry_received = True + self._warmup_count = 0 + self.get_logger().info('First ArduPilot pose received!') + self._build_telemetry_from_ardupilot() + + def _ap_twist_callback(self, msg: TwistStamped): + self._ap_twist = msg + self._build_telemetry_from_ardupilot() + + def _ap_imu_callback(self, msg: Imu): + self._ap_imu = msg + self._build_telemetry_from_ardupilot() + + def _ap_battery_callback(self, msg: BatteryState): + self._ap_battery = msg + self._build_telemetry_from_ardupilot() + + def _build_telemetry_from_ardupilot(self): + """Build telemetry dict from ArduPilot topics.""" + telemetry = {} + + # Position and orientation from pose + if self._ap_pose: + pos = self._ap_pose.pose.position + ori = self._ap_pose.pose.orientation + roll, pitch, yaw = quaternion_to_euler(ori.x, ori.y, ori.z, ori.w) + + telemetry['position'] = { + 'x': pos.x, + 'y': pos.y, + 'z': pos.z + } + telemetry['altimeter'] = { + 'altitude': pos.z, + 'vertical_velocity': self._ap_twist.twist.linear.z if self._ap_twist else 0.0 + } + telemetry['imu'] = { + 'orientation': {'roll': roll, 'pitch': pitch, 'yaw': yaw}, + 'angular_velocity': {'x': 0, 'y': 0, 'z': 0} + } + + # Velocity from twist + if self._ap_twist: + twist = self._ap_twist.twist + telemetry['velocity'] = { + 'x': twist.linear.x, + 'y': twist.linear.y, + 'z': twist.linear.z + } + if 'altimeter' in telemetry: + telemetry['altimeter']['vertical_velocity'] = twist.linear.z + + # Angular velocity from IMU + if self._ap_imu: + if 'imu' not in telemetry: + telemetry['imu'] = {} + telemetry['imu']['angular_velocity'] = { + 'x': self._ap_imu.angular_velocity.x, + 'y': self._ap_imu.angular_velocity.y, + 'z': self._ap_imu.angular_velocity.z + } + + # Battery + if self._ap_battery: + telemetry['battery'] = { + 'voltage': self._ap_battery.voltage, + 'remaining': self._ap_battery.percentage * 100 + } + + # Landing pad detection (placeholder - would need vision processing) + # For now, calculate from rover telemetry if available + if self._rover_telemetry and self._ap_pose: + rover_pos = self._rover_telemetry.get('position', {}) + rx = rover_pos.get('x', 0) + ry = rover_pos.get('y', 0) + dx = self._ap_pose.pose.position.x + dy = self._ap_pose.pose.position.y + dz = self._ap_pose.pose.position.z + + rel_x = rx - dx + rel_y = ry - dy + distance = math.sqrt(rel_x**2 + rel_y**2 + dz**2) + + telemetry['landing_pad'] = { + 'relative_x': rel_x, + 'relative_y': rel_y, + 'distance': distance, + 'confidence': 1.0 if distance < 10.0 else 0.0 + } + + self._latest_telemetry = telemetry + + # Legacy callbacks def _telemetry_callback(self, msg: String) -> None: try: self._latest_telemetry = json.loads(msg.data) @@ -211,11 +399,19 @@ class DroneController(Node): def main(args=None): - rclpy.init(args=args) + import sys + + # Check for --ardupilot flag + use_ardupilot = '--ardupilot' in sys.argv or '-a' in sys.argv + + # Remove our custom args before passing to rclpy + filtered_args = [a for a in (args or sys.argv) if a not in ['--ardupilot', '-a']] + + rclpy.init(args=filtered_args) controller = None try: - controller = DroneController() + controller = DroneController(use_ardupilot_topics=use_ardupilot) rclpy.spin(controller) except KeyboardInterrupt: print('\nShutting down...') diff --git a/gazebo/launch/ardupilot_drone.launch.py b/gazebo/launch/ardupilot_drone.launch.py index f9bb7c7..22809b5 100644 --- a/gazebo/launch/ardupilot_drone.launch.py +++ b/gazebo/launch/ardupilot_drone.launch.py @@ -1,40 +1,52 @@ #!/usr/bin/env python3 """ -ArduPilot SITL + Gazebo Launch File -Works with Gazebo Harmonic/Garden and ArduPilot SITL. +ArduPilot ROS 2 Launch Helper -This launch file: -1. Starts Gazebo with ArduPilot-compatible drone model -2. Sets up ROS-Gazebo bridge for telemetry -3. Optionally starts ArduPilot SITL +This file provides a helper for launching the official ArduPilot ROS 2 simulation. +For the full ArduPilot + Gazebo experience, use the official ardupilot_gz packages. -NOTE: ArduPilot SITL integration requires the ardupilot_gazebo plugin. -Install from: https://github.com/ArduPilot/ardupilot_gazebo +RECOMMENDED: Use the official launch files: + ros2 launch ardupilot_gz_bringup iris_runway.launch.py + +This local launch file is for custom Gazebo worlds or fallback testing. """ import os import shutil from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction +from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction, LogInfo from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): - """Generate the launch description.""" + """ + Generate launch description for ArduPilot simulation. + + This launch file: + 1. Checks for official ardupilot_gz packages + 2. Falls back to local Gazebo world if packages not found + 3. Starts ROS-Gazebo bridge for additional topics + """ # Get paths script_dir = os.path.dirname(os.path.abspath(__file__)) gazebo_dir = os.path.dirname(script_dir) - world_file = os.path.join(gazebo_dir, 'worlds', 'ardupilot_drone.sdf') + world_file = os.path.join(gazebo_dir, 'worlds', 'drone_landing.sdf') - # Check for ArduPilot Gazebo plugin - plugin_paths = [ - os.path.expanduser("~/ardupilot_gazebo/build"), - "/opt/ardupilot_gazebo/lib", - os.environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", ""), - ] - ardupilot_plugin_found = any(os.path.exists(p) for p in plugin_paths if p) + # Check if official ArduPilot packages are available + try: + import subprocess + result = subprocess.run( + ['ros2', 'pkg', 'prefix', 'ardupilot_gz_bringup'], + capture_output=True, text=True + ) + official_pkg_available = result.returncode == 0 + except Exception: + official_pkg_available = False + + # Use simulation time + use_sim_time = LaunchConfiguration('use_sim_time', default='true') # Determine Gazebo command if shutil.which('gz'): @@ -44,37 +56,35 @@ def generate_launch_description(): else: gz_cmd = ['gz', 'sim', '-r', world_file] - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - start_sitl = LaunchConfiguration('start_sitl', default='false') - - # Set plugin path for ArduPilot - env = os.environ.copy() - plugin_search_paths = ':'.join([p for p in plugin_paths if p and os.path.exists(p)]) - if plugin_search_paths: - env['GZ_SIM_SYSTEM_PLUGIN_PATH'] = plugin_search_paths + ':' + env.get('GZ_SIM_SYSTEM_PLUGIN_PATH', '') - actions = [ DeclareLaunchArgument( 'use_sim_time', default_value='true', description='Use simulation clock' ), - - DeclareLaunchArgument( - 'start_sitl', - default_value='false', - description='Start ArduPilot SITL automatically' - ), - - # Start Gazebo with ArduPilot world + ] + + if official_pkg_available: + # Use official ArduPilot packages + actions.append( + LogInfo(msg=[ + '\n', + '=' * 60, + '\n RECOMMENDED: Use official ArduPilot launch files:\n', + ' ros2 launch ardupilot_gz_bringup iris_runway.launch.py\n', + '=' * 60, + '\n', + ]) + ) + + # Start Gazebo with local world (for custom scenarios) + actions.extend([ ExecuteProcess( cmd=gz_cmd, output='screen', - additional_env=env ), - # ROS-Gazebo Bridge for telemetry and commands - # Delayed start to wait for Gazebo + # ROS-Gazebo Bridge TimerAction( period=2.0, actions=[ @@ -83,14 +93,13 @@ def generate_launch_description(): executable='parameter_bridge', name='gz_bridge', arguments=[ - # Rover velocity commands (ROS to Gazebo) + # Rover velocity commands '/rover/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist', - # Odometry (from Gazebo to ROS) - fallback if MAVLink not used + # Drone velocity commands + '/drone/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist', + # Odometry '/model/drone/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry', - # Camera (from Gazebo to ROS) - '/drone/camera@sensor_msgs/msg/Image[gz.msgs.Image', - # IMU (from Gazebo to ROS) - '/imu@sensor_msgs/msg/Imu[gz.msgs.IMU', + '/model/rover/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry', # Clock '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock', ], @@ -99,36 +108,31 @@ def generate_launch_description(): ), ] ), - ] + ]) return LaunchDescription(actions) if __name__ == '__main__': print("=" * 60) - print(" ArduPilot SITL + Gazebo Launch File") + print(" ArduPilot ROS 2 Launch Helper") print("=" * 60) print() - print("This is a ROS 2 launch file for ArduPilot integration.") + print("RECOMMENDED: Use official ArduPilot ROS 2 packages:") print() - print("Prerequisites:") - print(" 1. Gazebo (gz sim or ign gazebo)") - print(" 2. ros_gz_bridge package") - print(" 3. ArduPilot Gazebo plugin (optional, for SITL)") + print(" # Install ArduPilot ROS 2") + print(" ./setup/install_ardupilot.sh") print() - print("Usage:") + print(" # Source workspace") + print(" source ~/ardu_ws/install/setup.bash") + print() + print(" # Launch simulation (SITL + Gazebo + RViz)") + print(" ros2 launch ardupilot_gz_bringup iris_runway.launch.py") + print() + print(" # Connect MAVProxy") + print(" mavproxy.py --console --map --master=:14550") + print() + print("-" * 60) + print("Alternative: Use this local launch for custom worlds:") print(" ros2 launch gazebo/launch/ardupilot_drone.launch.py") print() - print("Then in another terminal:") - print(" # Start SITL") - print(" sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON") - print() - print(" # Or use the integrated runner") - print(" python run_ardupilot.py --no-sitl") - print() - print("Manual Gazebo Start:") - - if shutil.which('gz'): - print(" gz sim -r gazebo/worlds/ardupilot_drone.sdf") - else: - print(" ign gazebo gazebo/worlds/ardupilot_drone.sdf") diff --git a/gazebo/worlds/ardupilot_drone.sdf b/gazebo/worlds/ardupilot_drone.sdf deleted file mode 100644 index b1ff4ba..0000000 --- a/gazebo/worlds/ardupilot_drone.sdf +++ /dev/null @@ -1,504 +0,0 @@ - - - - - - - - 0.001 - 1.0 - 1000 - - - - - - - - ogre2 - - - - - - - - - true - 0 0 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - -0.5 0.1 -0.9 - - - - false - 0 0 10 0 0 0 - 0.3 0.3 0.3 1 - 0.0 0.0 0.0 1 - 0.5 0.3 -0.5 - - - - - true - - - - - 0 0 1 - 100 100 - - - - - - 1 - 1 - - - - - - - - 0 0 1 - 100 100 - - - - 0.5 0.5 0.5 1 - 0.7 0.7 0.7 1 - - - - - - - - 0 0 0.15 0 0 0 - - - - 10.0 - - 1.000 - 1.00 - 1.0 - - - - - - 1.0 1.0 0.3 - - - 0.1 0.5 0.1 1 - 0.2 0.7 0.2 1 - - - - - 1.0 1.0 0.3 - - - - - - 0 0 0.151 0 0 0 - 0.6 0.1 0.001 - - 1 1 1 1 - 1 1 1 1 - - - - -0.25 0 0.151 0 0 0 - 0.1 0.5 0.001 - - 1 1 1 1 - 1 1 1 1 - - - - 0.25 0 0.151 0 0 0 - 0.1 0.5 0.001 - - 1 1 1 1 - 1 1 1 1 - - - - - - - /rover/cmd_vel - 0 0 0 - - - - - - 0 0 0.194923 0 0 0 - - - - 0 0 0 0 0 0 - 1.5 - - 0.029125 - 0 - 0 - 0.029125 - 0 - 0.055225 - - - - - - 0.47 0.47 0.11 - - - - - - - 0.3 0.3 0.1 - - - 0.8 0.1 0.1 1 - 0.9 0.2 0.2 1 - - - - - - 0.12 0.12 0 0 0 0.785 - 0.2 0.02 0.02 - 0.3 0.3 0.3 1 - - - 0.12 -0.12 0 0 0 -0.785 - 0.2 0.02 0.02 - 0.3 0.3 0.3 1 - - - -0.12 0.12 0 0 0 -0.785 - 0.2 0.02 0.02 - 0.3 0.3 0.3 1 - - - -0.12 -0.12 0 0 0 0.785 - 0.2 0.02 0.02 - 0.3 0.3 0.3 1 - - - - - 0 0 0.08 0 0 0 - 0.020.04 - 0.2 0.2 0.8 1 - - - - - true - 400 - imu - - - 00.0002 - 00.0002 - 00.0002 - - - 00.017 - 00.017 - 00.017 - - - - - - - true - 50 - air_pressure - - 0 - - 0 - 0.01 - - - - - - - 0 0 -0.05 0 1.5708 0 - true - 30 - /drone/camera - - 1.047 - - 320 - 240 - R8G8B8 - - - 0.1 - 100 - - - - - - - - 0.13 -0.22 0.023 0 0 0 - - 0.025 - - 9.75e-0600 - 1.66704e-040 - 1.66704e-04 - - - - 0.10.005 - 0.1 0.1 0.1 1 - - - 0.10.005 - - - - - base_link - rotor_0 - - 0 0 1 - -1e161e16 - - - - - - -0.13 0.2 0.023 0 0 0 - - 0.025 - - 9.75e-0600 - 1.66704e-040 - 1.66704e-04 - - - - 0.10.005 - 0.1 0.1 0.1 1 - - - 0.10.005 - - - - - base_link - rotor_1 - - 0 0 1 - -1e161e16 - - - - - - 0.13 0.22 0.023 0 0 0 - - 0.025 - - 9.75e-0600 - 1.66704e-040 - 1.66704e-04 - - - - 0.10.005 - 0.1 0.1 0.1 1 - - - 0.10.005 - - - - - base_link - rotor_2 - - 0 0 1 - -1e161e16 - - - - - - -0.13 -0.2 0.023 0 0 0 - - 0.025 - - 9.75e-0600 - 1.66704e-040 - 1.66704e-04 - - - - 0.10.005 - 0.1 0.1 0.1 1 - - - 0.10.005 - - - - - base_link - rotor_3 - - 0 0 1 - -1e161e16 - - - - - - - 127.0.0.1 - 9002 - 127.0.0.1 - 9003 - - - 0 0 0 3.141593 0 0 - 0 0 0 3.141593 0 0 - - - imu_sensor - - - - - rotor_0_joint - VELOCITY - 0 - 838 - 0.2 - 0 - 0 - 0 - 0 - 2.5 - -2.5 - - - - - rotor_1_joint - VELOCITY - 0 - 838 - 0.2 - 0 - 0 - 0 - 0 - 2.5 - -2.5 - - - - - rotor_2_joint - VELOCITY - 0 - -838 - 0.2 - 0 - 0 - 0 - 0 - 2.5 - -2.5 - - - - - rotor_3_joint - VELOCITY - 0 - -838 - 0.2 - 0 - 0 - 0 - 0 - 2.5 - -2.5 - - - - - - odom - base_link - /model/drone/odometry - 50 - - - - - - diff --git a/mavlink_bridge.py b/mavlink_bridge.py index fdeb042..c894b13 100644 --- a/mavlink_bridge.py +++ b/mavlink_bridge.py @@ -1,579 +1,423 @@ #!/usr/bin/env python3 """ -MAVLink Bridge for ArduPilot SITL Integration. -Connects Gazebo simulation to ArduPilot SITL via MAVProxy. +MAVLink Command Interface for ArduPilot. -This bridge: -- Connects to ArduPilot SITL via MAVLink (UDP) -- Receives telemetry from ArduPilot (position, attitude, battery, etc.) -- Sends commands to ArduPilot (velocity, position, etc.) -- Publishes ROS 2 telemetry for the drone controller +This module provides a Python interface for sending MAVLink commands to ArduPilot +when using the official ROS 2 DDS integration. Telemetry is received via native +ROS 2 topics (/ap/*), but commands are still sent via MAVLink. + +Usage: + from mavlink_bridge import MAVLinkCommander + + cmd = MAVLinkCommander() + cmd.connect() + cmd.arm() + cmd.takeoff(5.0) + cmd.land() """ -import json -import math import time import threading -from typing import Optional, Dict, Any, Tuple - -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import Twist -from std_msgs.msg import String +from typing import Optional, Callable +from enum import Enum try: from pymavlink import mavutil - from pymavlink.dialects.v20 import ardupilotmega as mavlink - MAVLINK_AVAILABLE = True + PYMAVLINK_AVAILABLE = True except ImportError: - MAVLINK_AVAILABLE = False - print("WARNING: pymavlink not installed. Run: pip install pymavlink") + PYMAVLINK_AVAILABLE = False + print("Warning: pymavlink not installed. Install with: pip install pymavlink") -class MAVLinkBridge(Node): +class FlightMode(Enum): + """ArduCopter flight modes.""" + STABILIZE = 0 + ACRO = 1 + ALT_HOLD = 2 + AUTO = 3 + GUIDED = 4 + LOITER = 5 + RTL = 6 + CIRCLE = 7 + LAND = 9 + DRIFT = 11 + SPORT = 13 + FLIP = 14 + AUTOTUNE = 15 + POSHOLD = 16 + BRAKE = 17 + THROW = 18 + AVOID_ADSB = 19 + GUIDED_NOGPS = 20 + SMART_RTL = 21 + FLOWHOLD = 22 + FOLLOW = 23 + ZIGZAG = 24 + SYSTEMID = 25 + AUTOROTATE = 26 + + +class MAVLinkCommander: """ - ROS 2 Node that bridges between ROS topics and ArduPilot SITL via MAVLink. + MAVLink command interface for ArduPilot. + + Provides methods for arming, disarming, mode changes, and flight commands. """ - # MAVLink connection defaults - DEFAULT_SITL_HOST = "127.0.0.1" - DEFAULT_SITL_PORT = 14550 # MAVProxy output port - DEFAULT_SYSTEM_ID = 1 - DEFAULT_COMPONENT_ID = 191 # MAV_COMP_ID_MISSIONPLANNER - - # Telemetry rate (Hz) - TELEMETRY_RATE = 50.0 - - def __init__( - self, - sitl_host: str = None, - sitl_port: int = None, - system_id: int = None, - component_id: int = None - ): - super().__init__('mavlink_bridge') + def __init__(self, connection_string: str = "udpin:127.0.0.1:14550"): + """ + Initialize MAVLink commander. - # 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) + Args: + connection_string: MAVLink connection string (default for MAVProxy output) + """ + if not PYMAVLINK_AVAILABLE: + raise ImportError("pymavlink is required: pip install pymavlink") - self._sitl_host = self.get_parameter('sitl_host').value - self._sitl_port = self.get_parameter('sitl_port').value - self._system_id = self.get_parameter('system_id').value - self._component_id = self.get_parameter('component_id').value - - self.get_logger().info('=' * 60) - self.get_logger().info('MAVLink Bridge Starting...') - self.get_logger().info(f' Connecting to SITL at {self._sitl_host}:{self._sitl_port}') - self.get_logger().info('=' * 60) - - # MAVLink connection - self._mav_conn: Optional[mavutil.mavlink_connection] = None + self._connection_string = connection_string + self._mav: Optional[mavutil.mavlink_connection] = None self._connected = False self._armed = False self._flight_mode = "UNKNOWN" + self._heartbeat_thread: Optional[threading.Thread] = None + self._running = False - # Telemetry state - self._position = [0.0, 0.0, 0.0] # x, y, z (NED frame, converted to ENU) - self._velocity = [0.0, 0.0, 0.0] # vx, vy, vz - self._attitude = [0.0, 0.0, 0.0] # roll, pitch, yaw (radians) - self._angular_velocity = [0.0, 0.0, 0.0] # wx, wy, wz - self._battery_voltage = 0.0 - self._battery_remaining = 100 - self._gps_fix = 0 - self._landed = False - self._home_position = None - self._last_heartbeat = 0.0 + # Callbacks + self._on_heartbeat: Optional[Callable] = None + self._on_state_change: Optional[Callable] = None + + def connect(self, timeout: float = 30.0) -> bool: + """ + Connect to ArduPilot via MAVLink. - # Rover position (for landing pad detection) - self._rover_position = [0.0, 0.0, 0.15] - - # Thread safety - self._lock = threading.Lock() - - # ROS 2 Subscriptions - self._cmd_vel_sub = self.create_subscription( - Twist, '/cmd_vel', self._cmd_vel_callback, 10 - ) - self.get_logger().info(' Subscribed to: /cmd_vel') - - self._rover_sub = self.create_subscription( - String, '/rover/telemetry', self._rover_callback, 10 - ) - self.get_logger().info(' Subscribed to: /rover/telemetry') - - # ROS 2 Publishers - self._telemetry_pub = self.create_publisher( - String, '/drone/telemetry', 10 - ) - self.get_logger().info(' Publishing to: /drone/telemetry') - - self._status_pub = self.create_publisher( - String, '/mavlink/status', 10 - ) - self.get_logger().info(' Publishing to: /mavlink/status') - - # Initialize MAVLink connection - self._connect_mavlink() - - # Start telemetry thread - self._running = True - self._mavlink_thread = threading.Thread(target=self._mavlink_loop, daemon=True) - self._mavlink_thread.start() - - # Telemetry timer - telemetry_period = 1.0 / self.TELEMETRY_RATE - self._telemetry_timer = self.create_timer(telemetry_period, self._publish_telemetry) - - self.get_logger().info('MAVLink Bridge Ready!') - - def _connect_mavlink(self) -> bool: - """Establish MAVLink connection to ArduPilot SITL.""" - if not MAVLINK_AVAILABLE: - self.get_logger().error('pymavlink not available!') - return False + Args: + timeout: Connection timeout in seconds + + Returns: + True if connected successfully + """ + print(f"Connecting to {self._connection_string}...") try: - connection_string = f'udpin:{self._sitl_host}:{self._sitl_port}' - self.get_logger().info(f'Connecting to: {connection_string}') - - self._mav_conn = mavutil.mavlink_connection( - connection_string, - source_system=self._system_id, - source_component=self._component_id - ) + self._mav = mavutil.mavlink_connection(self._connection_string) # Wait for heartbeat - self.get_logger().info('Waiting for heartbeat...') - self._mav_conn.wait_heartbeat(timeout=30) + heartbeat = self._mav.wait_heartbeat(timeout=timeout) + if heartbeat is None: + print("Connection failed: No heartbeat received") + return False self._connected = True - self._last_heartbeat = time.time() - self.get_logger().info( - f'Connected! System: {self._mav_conn.target_system}, ' - f'Component: {self._mav_conn.target_component}' + self._running = True + + # Start heartbeat thread + self._heartbeat_thread = threading.Thread( + target=self._heartbeat_loop, daemon=True ) + self._heartbeat_thread.start() - # Request data streams - self._request_data_streams() - + print(f"Connected! System: {self._mav.target_system}, Component: {self._mav.target_component}") return True except Exception as e: - self.get_logger().error(f'MAVLink connection failed: {e}') - self._connected = False + print(f"Connection failed: {e}") return False - - def _request_data_streams(self): - """Request required data streams from ArduPilot.""" - if not self._mav_conn: - return - - # Request all data streams at high rate - streams = [ - (mavutil.mavlink.MAV_DATA_STREAM_ALL, 10), - (mavutil.mavlink.MAV_DATA_STREAM_RAW_SENSORS, 50), - (mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, 5), - (mavutil.mavlink.MAV_DATA_STREAM_POSITION, 50), - (mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, 50), # Attitude - (mavutil.mavlink.MAV_DATA_STREAM_EXTRA2, 10), # VFR_HUD - ] - - for stream_id, rate in streams: - self._mav_conn.mav.request_data_stream_send( - self._mav_conn.target_system, - self._mav_conn.target_component, - stream_id, - rate, - 1 # Start - ) - - def _mavlink_loop(self): - """Background thread for receiving MAVLink messages.""" - reconnect_delay = 2.0 - - while self._running: - if not self._connected: - time.sleep(reconnect_delay) - self._connect_mavlink() - continue - + + def disconnect(self): + """Disconnect from ArduPilot.""" + self._running = False + if self._heartbeat_thread: + self._heartbeat_thread.join(timeout=2.0) + if self._mav: + self._mav.close() + self._connected = False + print("Disconnected") + + def _heartbeat_loop(self): + """Background thread for receiving heartbeats and updating state.""" + while self._running and self._mav: try: - # Non-blocking message receive - msg = self._mav_conn.recv_match(blocking=True, timeout=0.1) - - if msg is None: - # Check for connection timeout - if time.time() - self._last_heartbeat > 5.0: - self.get_logger().warning('Heartbeat timeout, reconnecting...') - self._connected = False - continue - - self._process_mavlink_message(msg) - - except Exception as e: - self.get_logger().error(f'MAVLink receive error: {e}') - self._connected = False - - def _process_mavlink_message(self, msg): - """Process incoming MAVLink messages.""" - msg_type = msg.get_type() - - with self._lock: - if msg_type == 'HEARTBEAT': - self._last_heartbeat = time.time() - 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 - # ========================================================================= - + msg = self._mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1.0) + if msg: + self._armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED + self._flight_mode = mavutil.mode_string_v10(msg) + + if self._on_heartbeat: + self._on_heartbeat(self._armed, self._flight_mode) + except Exception: + pass + + @property + def connected(self) -> bool: + return self._connected + + @property + def armed(self) -> bool: + return self._armed + + @property + def flight_mode(self) -> str: + return self._flight_mode + def arm(self, force: bool = False) -> bool: - """Arm the drone.""" - if not self._connected or not self._mav_conn: + """ + Arm the vehicle. + + Args: + force: Force arm (bypass preflight checks) + + Returns: + True if arm command was sent + """ + if not self._connected: + print("Not connected") return False - self.get_logger().info('Arming drone...') - self._mav_conn.arducopter_arm() - return self._wait_for_arm(True, timeout=5.0) - + print("Arming...") + + param = 21196.0 if force else 0.0 # Force arm magic number + + self._mav.mav.command_long_send( + self._mav.target_system, + self._mav.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, # Confirmation + 1, # Arm + param, # Force + 0, 0, 0, 0, 0 + ) + + # Wait for confirmation + time.sleep(0.5) + return True + def disarm(self, force: bool = False) -> bool: - """Disarm the drone.""" - if not self._connected or not self._mav_conn: + """ + Disarm the vehicle. + + Args: + force: Force disarm + + Returns: + True if disarm command was sent + """ + if not self._connected: + print("Not connected") return False - self.get_logger().info('Disarming drone...') - self._mav_conn.arducopter_disarm() - return self._wait_for_arm(False, timeout=5.0) - - def _wait_for_arm(self, expected: bool, timeout: float = 5.0) -> bool: - """Wait for arm/disarm to complete.""" - start = time.time() - while time.time() - start < timeout: - if self._armed == expected: - return True - time.sleep(0.1) - return False - + print("Disarming...") + + param = 21196.0 if force else 0.0 + + self._mav.mav.command_long_send( + self._mav.target_system, + self._mav.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, + 0, # Disarm + param, + 0, 0, 0, 0, 0 + ) + + time.sleep(0.5) + return True + def set_mode(self, mode: str) -> bool: - """Set flight mode (e.g., 'GUIDED', 'LAND', 'LOITER').""" - if not self._connected or not self._mav_conn: + """ + Set flight mode. + + Args: + mode: Mode name (GUIDED, LAND, LOITER, etc.) + + Returns: + True if mode change command was sent + """ + if not self._connected: + print("Not connected") return False - self.get_logger().info(f'Setting mode to: {mode}') + mode_upper = mode.upper() + mode_id = self._mav.mode_mapping().get(mode_upper) - mode_mapping = self._mav_conn.mode_mapping() - if mode.upper() not in mode_mapping: - self.get_logger().error(f'Unknown mode: {mode}') + if mode_id is None: + print(f"Unknown mode: {mode}") return False - mode_id = mode_mapping[mode.upper()] - self._mav_conn.set_mode(mode_id) + print(f"Setting mode to {mode_upper}...") - # Wait for mode change - start = time.time() - while time.time() - start < 5.0: - if self._flight_mode.upper() == mode.upper(): - return True - time.sleep(0.1) + self._mav.mav.set_mode_send( + self._mav.target_system, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + mode_id + ) - return False - + time.sleep(0.5) + return True + def takeoff(self, altitude: float = 5.0) -> bool: - """Take off to specified altitude.""" - if not self._connected or not self._mav_conn: + """ + Command takeoff to specified altitude. + + Args: + altitude: Target altitude in meters + + Returns: + True if takeoff command was sent + """ + if not self._connected: + print("Not connected") return False - self.get_logger().info(f'Taking off to {altitude}m...') + print(f"Taking off to {altitude}m...") - # Set GUIDED mode - if not self.set_mode('GUIDED'): - self.get_logger().error('Failed to set GUIDED mode') - return False - - # Arm if not armed - if not self._armed: - if not self.arm(): - self.get_logger().error('Failed to arm') - return False - - # Command takeoff - self._mav_conn.mav.command_long_send( - self._mav_conn.target_system, - self._mav_conn.target_component, + self._mav.mav.command_long_send( + self._mav.target_system, + self._mav.target_component, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, - 0, # confirmation - 0, 0, 0, 0, # params 1-4 (unused) - 0, 0, # lat, lon (use current) - altitude # altitude + 0, + 0, 0, 0, 0, 0, 0, + altitude + ) + + return True + + def land(self) -> bool: + """ + Command landing. + + Returns: + True if land command was sent + """ + if not self._connected: + print("Not connected") + return False + + print("Landing...") + + self._mav.mav.command_long_send( + self._mav.target_system, + self._mav.target_component, + mavutil.mavlink.MAV_CMD_NAV_LAND, + 0, + 0, 0, 0, 0, 0, 0, 0 + ) + + return True + + def goto_position_ned(self, north: float, east: float, down: float) -> bool: + """ + Go to position in NED frame relative to home. + + Args: + north: North position (m) + east: East position (m) + down: Down position (m, negative is up) + + Returns: + True if command was sent + """ + if not self._connected: + print("Not connected") + return False + + print(f"Going to NED: ({north}, {east}, {down})") + + self._mav.mav.set_position_target_local_ned_send( + 0, # time_boot_ms + self._mav.target_system, + self._mav.target_component, + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + 0b0000111111111000, # Position only + north, east, down, # Position + 0, 0, 0, # Velocity + 0, 0, 0, # Acceleration + 0, 0 # Yaw, yaw_rate + ) + + return True + + def set_velocity_ned(self, vn: float, ve: float, vd: float, yaw_rate: float = 0.0) -> bool: + """ + Set velocity in NED frame. + + Args: + vn: North velocity (m/s) + ve: East velocity (m/s) + vd: Down velocity (m/s, positive is down) + yaw_rate: Yaw rate (rad/s) + + Returns: + True if command was sent + """ + if not self._connected: + return False + + self._mav.mav.set_position_target_local_ned_send( + 0, # time_boot_ms + self._mav.target_system, + self._mav.target_component, + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + 0b0000011111000111, # Velocity only + 0, 0, 0, # Position (ignored) + vn, ve, vd, # Velocity + 0, 0, 0, # Acceleration + 0, yaw_rate + ) + + return True + + def set_param(self, param_id: str, value: float) -> bool: + """ + Set a parameter value. + + Args: + param_id: Parameter name + value: Parameter value + + Returns: + True if command was sent + """ + if not self._connected: + print("Not connected") + return False + + print(f"Setting {param_id} = {value}") + + self._mav.mav.param_set_send( + self._mav.target_system, + self._mav.target_component, + param_id.encode('utf-8'), + value, + mavutil.mavlink.MAV_PARAM_TYPE_REAL32 ) return True - def land(self) -> bool: - """Command the drone to land.""" - if not self._connected or not self._mav_conn: - return False - - self.get_logger().info('Landing...') - return self.set_mode('LAND') - def goto_position(self, x: float, y: float, z: float, yaw: float = 0.0): - """Go to a local position (ENU frame).""" - if not self._connected or not self._mav_conn: - return - - # Convert ENU to NED - type_mask = ( - mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE | - mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE | - mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE | - mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | - mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | - mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | - mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE - ) - - self._mav_conn.mav.set_position_target_local_ned_send( - 0, # time_boot_ms - self._mav_conn.target_system, - self._mav_conn.target_component, - mavutil.mavlink.MAV_FRAME_LOCAL_NED, - type_mask, - y, x, -z, # ENU to NED position - 0, 0, 0, # velocity (ignored) - 0, 0, 0, # acceleration (ignored) - yaw, 0 # yaw, yaw_rate - ) - - def destroy_node(self): - """Clean shutdown.""" - self._running = False - if self._mavlink_thread.is_alive(): - self._mavlink_thread.join(timeout=2.0) - if self._mav_conn: - self._mav_conn.close() - super().destroy_node() - - -def main(args=None): - if not MAVLINK_AVAILABLE: - print("ERROR: pymavlink is required. Install with: pip install pymavlink") +def main(): + """Example usage of MAVLinkCommander.""" + if not PYMAVLINK_AVAILABLE: + print("pymavlink not installed") return - print("\n" + "=" * 60) - print(" MAVLink Bridge for ArduPilot SITL") - print("=" * 60 + "\n") + cmd = MAVLinkCommander() - rclpy.init(args=args) - bridge = None + if not cmd.connect(timeout=10.0): + print("Failed to connect") + return - try: - bridge = MAVLinkBridge() - rclpy.spin(bridge) - except KeyboardInterrupt: - print('\nShutting down...') - finally: - if bridge: - bridge.destroy_node() - if rclpy.ok(): - rclpy.shutdown() + print(f"Armed: {cmd.armed}") + print(f"Mode: {cmd.flight_mode}") + + # Example commands (uncomment to use): + # cmd.set_mode("GUIDED") + # cmd.arm() + # cmd.takeoff(5.0) + # time.sleep(10) + # cmd.land() + + input("Press Enter to disconnect...") + cmd.disconnect() if __name__ == '__main__': diff --git a/requirements.txt b/requirements.txt index a4d1dd4..62b4dd1 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,5 +1,16 @@ +# Drone Simulation Requirements +# Install with: pip install -r requirements.txt + +# Core simulation pybullet>=3.2.5 numpy>=1.20.0 pillow>=9.0.0 -pyinstaller>=6.0.0 + +# Camera viewer +opencv-python>=4.5.0 + +# ArduPilot/MAVLink (optional - for ArduPilot mode) pymavlink>=2.4.0 + +# Build executables (optional) +pyinstaller>=6.0.0 diff --git a/run_ardupilot.py b/run_ardupilot.py index 621a68c..0cacc88 100644 --- a/run_ardupilot.py +++ b/run_ardupilot.py @@ -1,16 +1,20 @@ #!/usr/bin/env python3 """ -Run ArduPilot SITL + Gazebo Simulation. +ArduPilot ROS 2 Launcher - Official DDS Integration. -This script orchestrates the full ArduPilot SITL + Gazebo simulation stack: -1. Launches ArduPilot SITL (sim_vehicle.py with Gazebo model) -2. Starts MAVProxy for GCS connectivity -3. Runs MAVLink bridge + controllers +This script provides a convenient way to launch the ArduPilot SITL simulation +using the official ardupilot_gz packages with ROS 2 DDS support. Usage: - python run_ardupilot.py # Start everything - python run_ardupilot.py --pattern circular # With rover movement - python run_ardupilot.py --no-sitl # Just controllers (SITL already running) + python run_ardupilot.py # Launch Iris on runway + python run_ardupilot.py --world maze # Launch Iris in maze + python run_ardupilot.py --vehicle rover # Launch WildThumper rover + python run_ardupilot.py --mavproxy # Also start MAVProxy + +Prerequisites: + - ArduPilot ROS 2 packages installed (./setup/install_ardupilot.sh) + - ROS 2 Humble/Jazzy sourced + - ~/ardu_ws workspace built and sourced """ import argparse @@ -21,336 +25,208 @@ import sys import time from pathlib import Path -import rclpy -from rclpy.executors import MultiThreadedExecutor -# Check for pymavlink -try: - from pymavlink import mavutil - MAVLINK_AVAILABLE = True -except ImportError: - MAVLINK_AVAILABLE = False - -from mavlink_bridge import MAVLinkBridge -from drone_controller import DroneController -from rover_controller import RoverController, MovementPattern +def check_ros2(): + """Check if ROS 2 is available.""" + try: + subprocess.run(['ros2', '--help'], capture_output=True, check=True) + return True + except (subprocess.CalledProcessError, FileNotFoundError): + return False -def find_sim_vehicle() -> str: - """Find sim_vehicle.py in common ArduPilot locations.""" - locations = [ - os.path.expanduser("~/ardupilot/Tools/autotest/sim_vehicle.py"), - "/opt/ardupilot/Tools/autotest/sim_vehicle.py", - os.environ.get("ARDUPILOT_HOME", "") + "/Tools/autotest/sim_vehicle.py", +def check_ardupilot_packages(): + """Check if ArduPilot ROS 2 packages are installed.""" + try: + result = subprocess.run( + ['ros2', 'pkg', 'list'], + capture_output=True, + text=True, + check=True + ) + packages = result.stdout + return 'ardupilot_gz_bringup' in packages or 'ardupilot_sitl' in packages + except (subprocess.CalledProcessError, FileNotFoundError): + return False + + +def source_workspace(): + """Source the ArduPilot workspace.""" + ardu_ws = os.path.expanduser("~/ardu_ws") + setup_bash = os.path.join(ardu_ws, "install", "setup.bash") + + if os.path.exists(setup_bash): + # Update environment by sourcing the workspace + # This is done by running commands in a sourced shell + return True + return False + + +def get_launch_command(world: str, vehicle: str) -> list: + """Get the appropriate launch command.""" + + launch_files = { + # Copter configurations + 'runway': ('ardupilot_gz_bringup', 'iris_runway.launch.py'), + 'maze': ('ardupilot_gz_bringup', 'iris_maze.launch.py'), + 'iris': ('ardupilot_gz_bringup', 'iris_runway.launch.py'), + + # Rover configurations + 'rover': ('ardupilot_gz_bringup', 'wildthumper_playpen.launch.py'), + 'wildthumper': ('ardupilot_gz_bringup', 'wildthumper_playpen.launch.py'), + + # SITL only (no Gazebo) + 'sitl': ('ardupilot_sitl', 'sitl_dds_udp.launch.py'), + } + + if vehicle == 'rover': + key = 'rover' + else: + key = world if world in launch_files else 'runway' + + package, launch_file = launch_files.get(key, launch_files['runway']) + + cmd = ['ros2', 'launch', package, launch_file] + + # Add SITL-specific parameters if using sitl_dds_udp + if launch_file == 'sitl_dds_udp.launch.py': + cmd.extend([ + 'transport:=udp4', + 'synthetic_clock:=True', + 'model:=quad' if vehicle == 'copter' else 'rover', + 'speedup:=1', + ]) + + return cmd + + +def launch_mavproxy(master_port: int = 14550): + """Launch MAVProxy in a new terminal.""" + mavproxy_cmd = f"mavproxy.py --console --map --master=:{master_port}" + + # Try different terminal emulators + terminals = [ + ['gnome-terminal', '--', 'bash', '-c', mavproxy_cmd], + ['xterm', '-e', mavproxy_cmd], + ['konsole', '-e', mavproxy_cmd], ] - for loc in locations: - if os.path.exists(loc): - return loc + for term_cmd in terminals: + try: + subprocess.Popen(term_cmd) + return True + except FileNotFoundError: + continue - # Try finding it in PATH - import shutil - if shutil.which("sim_vehicle.py"): - return "sim_vehicle.py" - - return None + print(f"[WARN] Could not open terminal for MAVProxy") + print(f"[INFO] Run manually: {mavproxy_cmd}") + return False def parse_args(): parser = argparse.ArgumentParser( - description='Run ArduPilot SITL + Gazebo + MAVLink Bridge', + description='Launch ArduPilot SITL with ROS 2 and Gazebo', formatter_class=argparse.RawDescriptionHelpFormatter, epilog=""" Examples: - python run_ardupilot.py # Full stack with Gazebo - python run_ardupilot.py --pattern circular # Moving rover - python run_ardupilot.py --no-sitl # SITL already running - python run_ardupilot.py --mavproxy-port 14551 # Custom port + python run_ardupilot.py # Iris on runway + python run_ardupilot.py --world maze # Iris in maze + python run_ardupilot.py --vehicle rover # WildThumper rover + python run_ardupilot.py --mavproxy # With MAVProxy + +Available worlds: runway, maze, sitl (no Gazebo) +Available vehicles: copter, rover """ ) - # SITL options parser.add_argument( - '--no-sitl', action='store_true', - help='Skip launching SITL (assume already running)' + '--world', '-w', type=str, default='runway', + choices=['runway', 'maze', 'sitl'], + help='Simulation world (default: runway)' ) parser.add_argument( - '--sitl-path', type=str, - help='Path to sim_vehicle.py' + '--vehicle', '-v', type=str, default='copter', + choices=['copter', 'rover'], + help='Vehicle type (default: copter)' ) parser.add_argument( - '--vehicle', type=str, default='ArduCopter', - choices=['ArduCopter', 'ArduPlane', 'APMrover2'], - help='Vehicle type (default: ArduCopter)' + '--mavproxy', '-m', action='store_true', + help='Also launch MAVProxy in a new terminal' ) - parser.add_argument( - '--frame', type=str, default='gazebo-iris', - help='Vehicle frame (default: gazebo-iris)' - ) - - # MAVProxy settings parser.add_argument( '--mavproxy-port', type=int, default=14550, - help='MAVProxy output port (default: 14550)' - ) - parser.add_argument( - '--sitl-port', type=int, default=5760, - help='SITL connection port (default: 5760)' + help='MAVProxy master port (default: 14550)' ) - # Rover options - parser.add_argument( - '--pattern', type=str, default='stationary', - choices=['stationary', 'linear', 'circular', 'random', 'square'], - help='Rover movement pattern (default: stationary)' - ) - parser.add_argument( - '--speed', '-s', type=float, default=0.5, - help='Rover speed in m/s (default: 0.5)' - ) - parser.add_argument( - '--amplitude', '-a', type=float, default=2.0, - help='Rover movement amplitude in meters (default: 2.0)' - ) - parser.add_argument( - '--no-rover', action='store_true', - help='Disable rover controller' - ) - - # Other - parser.add_argument( - '--console', action='store_true', - help='Show MAVProxy console' - ) - parser.add_argument( - '--map', action='store_true', - help='Show MAVProxy map' - ) - - args, _ = parser.parse_known_args() - return args - - -class SITLProcess: - """Manages the ArduPilot SITL process.""" - - def __init__( - self, - sim_vehicle_path: str, - vehicle: str = 'ArduCopter', - frame: str = 'gazebo-iris', - sitl_port: int = 5760, - mavproxy_port: int = 14550, - console: bool = False, - show_map: bool = False - ): - self.sim_vehicle_path = sim_vehicle_path - self.vehicle = vehicle - self.frame = frame - self.sitl_port = sitl_port - self.mavproxy_port = mavproxy_port - self.console = console - self.show_map = show_map - self.process = None - - def start(self) -> bool: - """Start SITL process.""" - if not self.sim_vehicle_path: - print("ERROR: sim_vehicle.py not found!") - print("Please set --sitl-path or ARDUPILOT_HOME environment variable") - return False - - cmd = [ - sys.executable, self.sim_vehicle_path, - '-v', self.vehicle, - '-f', self.frame, - '--model', 'JSON', - '--sitl-instance-args', f'--sim-address=127.0.0.1', - '--out', f'udp:127.0.0.1:{self.mavproxy_port}', - ] - - if self.console: - cmd.append('--console') - if self.show_map: - cmd.append('--map') - - print(f"Starting SITL: {' '.join(cmd)}") - - try: - self.process = subprocess.Popen( - cmd, - stdout=subprocess.PIPE, - stderr=subprocess.STDOUT - ) - return True - except Exception as e: - print(f"Failed to start SITL: {e}") - return False - - def stop(self): - """Stop SITL process.""" - if self.process: - self.process.terminate() - self.process.wait(timeout=5.0) - self.process = None - - -def wait_for_sitl(host: str = '127.0.0.1', port: int = 14550, timeout: float = 60.0) -> bool: - """Wait for SITL to be ready.""" - print(f"Waiting for SITL on {host}:{port}...") - start = time.time() - - while time.time() - start < timeout: - try: - conn = mavutil.mavlink_connection(f'udpin:{host}:{port}', timeout=1) - if conn.wait_heartbeat(timeout=1): - print("SITL is ready!") - conn.close() - return True - except Exception: - pass - time.sleep(1) - - print("Timeout waiting for SITL") - return False + return parser.parse_args() def main(): args = parse_args() - if not MAVLINK_AVAILABLE: - print("ERROR: pymavlink is required!") - print("Install with: pip install pymavlink") - return 1 - print("=" * 60) - print(" ArduPilot SITL + Gazebo Simulation") - print("=" * 60) - print(f" Vehicle: {args.vehicle}") - print(f" Frame: {args.frame}") - print(f" Rover Pattern: {args.pattern}") - print(f" MAVProxy Port: {args.mavproxy_port}") + print(" ArduPilot SITL + Gazebo (Official ROS 2 DDS)") print("=" * 60) print() - sitl = None - - # Start SITL if not skipped - if not args.no_sitl: - sim_vehicle = args.sitl_path or find_sim_vehicle() - - if not sim_vehicle: - print("=" * 60) - print("WARNING: sim_vehicle.py not found!") - print() - 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() + # Check ROS 2 + if not check_ros2(): + print("[ERROR] ROS 2 not found!") + print("Please source ROS 2:") + print(" source /opt/ros/humble/setup.bash") return 1 - # Initialize ROS - rclpy.init() + print("[OK] ROS 2 available") - nodes = [] - executor = MultiThreadedExecutor(num_threads=4) + # Check ArduPilot packages + 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: - # Create MAVLink bridge - bridge = MAVLinkBridge( - sitl_port=args.mavproxy_port - ) - nodes.append(bridge) - executor.add_node(bridge) - print("[OK] MAVLink Bridge started") - - # Create drone controller - drone = DroneController() - nodes.append(drone) - executor.add_node(drone) - print("[OK] Drone Controller started") - - # Create rover controller - if not args.no_rover: - pattern = MovementPattern[args.pattern.upper()] - rover = RoverController( - pattern=pattern, - speed=args.speed, - amplitude=args.amplitude - ) - nodes.append(rover) - executor.add_node(rover) - print("[OK] Rover Controller started") - - 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.") + subprocess.run(launch_cmd, check=True) + except subprocess.CalledProcessError as e: + print(f"[ERROR] Launch failed: {e}") + return 1 + except KeyboardInterrupt: + print("\nShutdown complete.") return 0 diff --git a/setup/install_ardupilot.sh b/setup/install_ardupilot.sh new file mode 100755 index 0000000..7b36fca --- /dev/null +++ b/setup/install_ardupilot.sh @@ -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 "" diff --git a/setup/install_ubuntu.sh b/setup/install_ubuntu.sh index bcaeb0e..38e486f 100755 --- a/setup/install_ubuntu.sh +++ b/setup/install_ubuntu.sh @@ -3,9 +3,11 @@ # Drone Simulation - Ubuntu/Debian Installation Script # ============================================================================= # Installs ROS 2, Gazebo, PyBullet, and all required dependencies -# Includes optional ArduPilot SITL setup for realistic flight controller +# Use --with-ardupilot to also install ArduPilot SITL # -# Usage: ./install_ubuntu.sh [--with-ardupilot] +# Usage: +# ./install_ubuntu.sh # Basic installation +# ./install_ubuntu.sh --with-ardupilot # Include ArduPilot SITL # ============================================================================= set -e @@ -242,62 +244,20 @@ python3 -c "from PIL import Image; print(' Pillow: OK')" || echo " Pillow: FAI python3 -c "from pymavlink import mavutil; print(' pymavlink: OK')" || echo " pymavlink: FAILED" # ----------------------------------------------------------------------------- -# Step 9: Optional ArduPilot SITL Installation +# Step 9: ArduPilot SITL Installation (if requested) # ----------------------------------------------------------------------------- if [ "$INSTALL_ARDUPILOT" = true ]; then echo "" echo "[STEP 9] Installing ArduPilot SITL..." + echo "[INFO] Calling dedicated ArduPilot install script..." - # Install ArduPilot dependencies - sudo apt-get install -y \ - python3-dev \ - python3-opencv \ - python3-wxgtk4.0 \ - python3-matplotlib \ - python3-lxml \ - libxml2-dev \ - libxslt1-dev || true - - # Clone ArduPilot if not exists - if [ ! -d "$HOME/ardupilot" ]; then - echo "[INFO] Cloning ArduPilot..." - git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git "$HOME/ardupilot" - cd "$HOME/ardupilot" - Tools/environment_install/install-prereqs-ubuntu.sh -y - cd "$PROJECT_ROOT" + # Call the dedicated ArduPilot install script + if [ -f "$SCRIPT_DIR/install_ardupilot.sh" ]; then + bash "$SCRIPT_DIR/install_ardupilot.sh" else - echo "[INFO] ArduPilot already exists at $HOME/ardupilot" + echo "[ERROR] install_ardupilot.sh not found!" + echo "[INFO] Please run: ./setup/install_ardupilot.sh" fi - - # Clone ArduPilot Gazebo plugin if not exists - if [ ! -d "$HOME/ardupilot_gazebo" ]; then - echo "[INFO] Cloning ArduPilot Gazebo plugin..." - git clone https://github.com/ArduPilot/ardupilot_gazebo.git "$HOME/ardupilot_gazebo" - cd "$HOME/ardupilot_gazebo" - mkdir -p build && cd build - cmake .. -DCMAKE_BUILD_TYPE=Release - make -j$(nproc) - cd "$PROJECT_ROOT" - else - echo "[INFO] ArduPilot Gazebo plugin already exists at $HOME/ardupilot_gazebo" - fi - - # Add to bashrc - if ! grep -q "ARDUPILOT_HOME" ~/.bashrc; then - echo '' >> ~/.bashrc - echo '# ArduPilot SITL' >> ~/.bashrc - echo 'export ARDUPILOT_HOME=$HOME/ardupilot' >> ~/.bashrc - echo 'export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest' >> ~/.bashrc - fi - - if ! grep -q "ardupilot_gazebo" ~/.bashrc; then - echo '' >> ~/.bashrc - echo '# ArduPilot Gazebo Plugin' >> ~/.bashrc - echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH' >> ~/.bashrc - echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH' >> ~/.bashrc - fi - - echo "[INFO] ArduPilot SITL installed" fi echo ""