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

Ardupilot Controller Script

Browse Source
This commit is contained in:
default
2026-01-04 02:31:31 +00:00
parent 4b514f1dd9
commit c5b208c91a
6 changed files with 438 additions and 349 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
README.md
View File

@@ -22,32 +22,25 @@ ros2 launch gazebo/launch/drone_landing.launch.py
```bash ```bash
source activate.sh source activate.sh
python run_gazebo.py --pattern circular python run_gazebo.py --pattern circular
python camera_viewer.py # View camera
``` ```
### ArduPilot GPS-Denied (2 Terminals) ### ArduPilot GPS-Denied (2 Terminals)
**Terminal 1:** **Terminal 1 - Gazebo:**
```bash ```bash
./scripts/run_ardupilot_sim.sh runway ./scripts/run_ardupilot_sim.sh runway
# Options: runway, warehouse, zephyr
``` ```
**Terminal 2:** **Terminal 2 - Controller:**
```bash ```bash
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console ./scripts/run_ardupilot_controller.sh
# GPS-denied mode:
param set ARMING_CHECK 0
mode stabilize
arm throttle force
``` ```
## Installation ## Installation
```bash ```bash
./setup/install_ubuntu.sh ./setup/install_ubuntu.sh
./setup/install_ardupilot.sh # Optional ./setup/install_ardupilot.sh # Optional for ArduPilot
source activate.sh source activate.sh
``` ```
@@ -55,12 +48,12 @@ source activate.sh
| File | Description | | File | Description |
|------|-------------| |------|-------------|
| `standalone_simulation.py` | All-in-one simulation | | `drone_controller.py` | **Your landing algorithm (used everywhere)** |
| `run_gazebo.py` | Gazebo controllers | | `standalone_simulation.py` | PyBullet simulation |
| `scripts/run_ardupilot_sim.sh` | ArduPilot launcher (auto GPU) | | `run_gazebo.py` | Gazebo + ROS 2 interface |
| `run_ardupilot.py` | ArduPilot + MAVLink interface |
| `config.py` | Configuration settings |
| `camera_viewer.py` | Camera feed window | | `camera_viewer.py` | Camera feed window |
| `drone_controller.py` | **Your landing algorithm** |
| `config.py` | Configuration |
## Sensors (GPS-Denied) ## Sensors (GPS-Denied)
@@ -71,16 +64,10 @@ source activate.sh
| Camera | Downward image | | Camera | Downward image |
| Landing Pad | Relative position (when visible) | | Landing Pad | Relative position (when visible) |
## Options ## Documentation
```bash
--pattern stationary, linear, circular, square, random
--speed Speed in m/s (default: 0.5)
```
## Docs
- [Installation](docs/installation.md) - [Installation](docs/installation.md)
- [Architecture](docs/architecture.md) - [Architecture](docs/architecture.md)
- [ArduPilot](docs/ardupilot.md) - [ArduPilot Guide](docs/ardupilot.md)
- [Gazebo](docs/gazebo.md) - [Gazebo Guide](docs/gazebo.md)
- [Drone Logic Guide](docs/drone_guide.md)

66
docs/architecture.md
View File

@@ -24,14 +24,20 @@ Terminal 1 Terminal 2
└───────────────────┘ ROS └───────────────────┘ └───────────────────┘ ROS └───────────────────┘
``` ```
### 3. ArduPilot GPS-Denied (2 Terminals) ### 3. ArduPilot (2 Terminals)
``` ```
Terminal 1 Terminal 2 Terminal 1 Terminal 2
┌───────────────────┐ ┌───────────────────┐ ┌───────────────────┐ ┌────────────────────────────┐
│ Gazebo + │◄──────►│ ArduPilot SITL │ │ Gazebo + │◄──────►│ run_ardupilot_controller.sh│
│ ArduPilot Plugin │ JSON │ + MAVProxy │ │ ArduPilot Plugin │ JSON │ ┌──────────────────┐ │
└───────────────────┘ └───────────────────┘ └───────────────────┘ │ │ ArduPilot SITL │ │
│ └─────────┬────────┘ │
│ │ MAVLink │
│ ┌─────────▼────────┐ │
│ │ run_ardupilot.py │ │
│ └──────────────────┘ │
└────────────────────────────┘
``` ```
## Data Flow ## Data Flow
@@ -48,31 +54,45 @@ Controller → /cmd_vel → Gazebo → /odometry → Controller
### ArduPilot ### ArduPilot
``` ```
Gazebo ◄─── JSON/UDP ───► SITL ◄─── MAVLink ───► MAVProxy Gazebo ◄─── JSON ───► SITL ◄─── MAVLink ───► Controller
``` ```
## Key Files ## Key Files
| File | Purpose | | File | Purpose |
|------|---------| |------|---------|
| `drone_controller.py` | Your landing algorithm | | `drone_controller.py` | **Your landing algorithm (used in ALL modes)** |
| `gazebo_bridge.py` | Gazebo ↔ ROS bridge | | `run_ardupilot.py` | MAVLink interface for ArduPilot |
| `camera_viewer.py` | Camera display | | `run_gazebo.py` | ROS 2 interface for Gazebo |
| `config.py` | Configuration | | `standalone_simulation.py` | PyBullet simulation engine |
| `config.py` | Shared configuration |
## GPS-Denied Sensors ## GPS-Denied Sensors
| Sensor | Data | The controller receives this standardized telemetry structure in all modes:
|--------|------|
| IMU | Orientation, angular velocity |
| Altimeter | Altitude, vertical velocity |
| Camera | Downward image |
| Landing Pad | Relative position |
## Topics ```python
telemetry = {
| Topic | Direction | "altimeter": {
|-------|-----------| "altitude": float, # Meters
| `/cmd_vel` | → Drone commands | "vertical_velocity": float # m/s (positive = up)
| `/drone/telemetry` | ← Sensor data | },
| `/drone/camera` | ← Camera images | "velocity": { # Body or Local frame
"x": float,
"y": float,
"z": float
},
"imu": {
"orientation": {
"roll": float,
"pitch": float,
"yaw": float
}
},
"landing_pad": { # If visible (None otherwise)
"relative_x": float,
"relative_y": float,
"distance": float
}
}
```

111
docs/ardupilot.md
View File

@@ -1,9 +1,38 @@
# ArduPilot GPS-Denied Simulation # ArduPilot GPS-Denied Simulation
Realistic flight controller simulation without GPS. Realistic flight controller simulation with your drone logic.
## Quick Start (2 Terminals) ## Quick Start (2 Terminals)
**Terminal 1 - Gazebo:**
```bash
./scripts/run_ardupilot_sim.sh runway
# Options: runway, warehouse, zephyr
```
**Terminal 2 - Controller + SITL:**
```bash
./scripts/run_ardupilot_controller.sh
```
## How It Works
The `run_ardupilot_controller.sh` script starts ArduPilot SITL in the background and connects your controller to it via MAVLink.
```
┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐
│ Gazebo │◄───►│ ArduPilot SITL │◄───►│ run_ardupilot.py│
│ (Physics) │JSON │ (Hidden) │MAV │ (Your Logic) │
└─────────────────┘ └─────────────────┘ └─────────────────┘
│
▼
drone_controller.py
```
## Manual Mode (Debugging)
If you need to debug with MAVProxy console (3 Terminals):
**Terminal 1:** **Terminal 1:**
```bash ```bash
./scripts/run_ardupilot_sim.sh runway ./scripts/run_ardupilot_sim.sh runway
@@ -12,10 +41,13 @@ Realistic flight controller simulation without GPS.
**Terminal 2:** **Terminal 2:**
```bash ```bash
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
```
# GPS-denied mode: **Terminal 3:**
```bash
# Debug commands in MAVProxy:
param set ARMING_CHECK 0 param set ARMING_CHECK 0
mode stabilize mode guided
arm throttle force arm throttle force
``` ```
@@ -26,67 +58,32 @@ arm throttle force
source ~/.bashrc source ~/.bashrc
``` ```
## World Options ## Configuration
```bash Your `drone_controller.py` receives telemetry and returns control inputs.
./scripts/run_ardupilot_sim.sh runway # Iris on runway (default)
./scripts/run_ardupilot_sim.sh warehouse # Iris in warehouse
./scripts/run_ardupilot_sim.sh zephyr # Zephyr plane
./scripts/run_ardupilot_sim.sh gimbal # Gimbal test
./scripts/run_ardupilot_sim.sh parachute # Parachute test
```
## GPS-Denied Configuration The simulation translates your inputs:
- `thrust` → Vertical velocity
In MAVProxy console: - `pitch/roll` → Horizontal velocity
- `yaw` → Yaw rate
```bash
param set ARMING_CHECK 0 # Disable pre-arm checks
mode stabilize # Manual with stabilization
arm throttle force # Force arm
```
## GPU Support
Auto-detects: NVIDIA > Intel > AMD > Software
Check your GPU:
```bash
glxinfo | grep "OpenGL renderer"
```
## MAVProxy Commands
```bash
mode stabilize # Manual mode
mode alt_hold # Altitude hold
arm throttle force # Arm motors
takeoff 5 # Takeoff 5m
mode land # Land
```
## Troubleshooting ## Troubleshooting
### "SITL failed to start"
Check if `sim_vehicle.py` is in your PATH:
```bash
export PATH=$PATH:~/ardupilot/Tools/autotest
```
### Drone drift
ArduPilot in GUIDED mode requires good position estimation. Without GPS, it relies on optical flow or visual odometry (not yet implemented in default setup). The drone might drift if relying only on IMU.
### "No JSON sensor message" ### "No JSON sensor message"
Ensure Gazebo (Terminal 1) is running before starting the controller.
Gazebo isn't sending data to SITL. Check: ## Visualizing Camera
1. Start Gazebo FIRST, then SITL
2. Plugin path is set:
```bash
echo $GZ_SIM_SYSTEM_PLUGIN_PATH | grep ardupilot
```
### Simulation laggy
Check GPU is being used (not software):
```bash
glxinfo | grep "OpenGL renderer"
# Should NOT show "llvmpipe"
```
### Can't arm
```bash ```bash
param set ARMING_CHECK 0 python camera_viewer.py --topic /drone/camera
arm throttle force
``` ```
(Requires bridging the topic if using ROS 2 bridge)

55
docs/drone_guide.md
View File

@@ -7,12 +7,13 @@ Implement your GPS-denied landing algorithm.
1. Edit `drone_controller.py` 1. Edit `drone_controller.py`
2. Find `calculate_landing_maneuver()` 2. Find `calculate_landing_maneuver()`
3. Implement your algorithm 3. Implement your algorithm
4. Test: `python standalone_simulation.py` 4. Test with any simulation mode
## Function to Implement ## Function to Implement
```python ```python
def calculate_landing_maneuver(self, telemetry, rover_telemetry): def calculate_landing_maneuver(self, telemetry, rover_telemetry):
# Your logic here
return (thrust, pitch, roll, yaw) return (thrust, pitch, roll, yaw)
``` ```
@@ -38,12 +39,16 @@ if landing_pad:
| Value | Range | Effect | | Value | Range | Effect |
|-------|-------|--------| |-------|-------|--------|
| thrust | ±1.0 | Up/down | | thrust | ±1.0 | Up/down (positive = up) |
| pitch | ±0.5 | Forward/back | | pitch | ±0.5 | Forward/back |
| roll | ±0.5 | Left/right | | roll | ±0.5 | Left/right |
| yaw | ±0.5 | Rotation | | yaw | ±0.5 | Rotation |
## Example Algorithm Note: In ArduPilot mode, these are scaled to velocities:
- Thrust → Z velocity
- Pitch/Roll → X/Y velocity
## Example Algorithm (PD Control)
```python ```python
def calculate_landing_maneuver(self, telemetry, rover_telemetry): def calculate_landing_maneuver(self, telemetry, rover_telemetry):
@@ -51,48 +56,18 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry):
altitude = alt.get('altitude', 5.0) altitude = alt.get('altitude', 5.0)
vert_vel = alt.get('vertical_velocity', 0.0) vert_vel = alt.get('vertical_velocity', 0.0)
vel = telemetry.get('velocity', {})
vel_x = vel.get('x', 0.0)
vel_y = vel.get('y', 0.0)
pad = telemetry.get('landing_pad')
# Altitude PD control # Altitude PD control
thrust = 0.5 * (0 - altitude) - 0.3 * vert_vel thrust = 0.5 * (target_alt - altitude) - 0.3 * vert_vel
# Horizontal control # Horizontal control
pad = telemetry.get('landing_pad')
if pad: if pad:
pitch = 0.3 * pad['relative_x'] - 0.2 * vel_x pitch = 0.3 * pad['relative_x']
roll = 0.3 * pad['relative_y'] - 0.2 * vel_y roll = 0.3 * pad['relative_y']
else: else:
pitch = -0.2 * vel_x # Hover
roll = -0.2 * vel_y pitch = 0
roll = 0
return (thrust, pitch, roll, 0.0) return (thrust, pitch, roll, 0.0)
``` ```
## Testing
```bash
# Easy
python standalone_simulation.py --pattern stationary
# Medium
python standalone_simulation.py --pattern circular --speed 0.3
# Hard
python standalone_simulation.py --pattern random --speed 0.5
```
## Configuration
Edit `config.py`:
```python
CONTROLLER = {
"Kp_z": 0.5,
"Kd_z": 0.3,
"Kp_xy": 0.3,
"Kd_xy": 0.2,
}
```

455
run_ardupilot.py
View File

@@ -1,235 +1,284 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
ArduPilot ROS 2 Launcher - Official DDS Integration. ArduPilot Drone Controller
==========================
This script provides a convenient way to launch the ArduPilot SITL simulation Connects your drone_controller.py logic to ArduPilot SITL via MAVLink.
using the official ardupilot_gz packages with ROS 2 DDS support.
Usage: Usage:
python run_ardupilot.py # Launch Iris on runway Terminal 1: ./scripts/run_ardupilot_sim.sh runway
python run_ardupilot.py --world maze # Launch Iris in maze Terminal 2: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
python run_ardupilot.py --vehicle rover # Launch WildThumper rover Terminal 3: python run_ardupilot.py
python run_ardupilot.py --mavproxy # Also start MAVProxy
Prerequisites: This script:
- ArduPilot ROS 2 packages installed (./setup/install_ardupilot.sh) 1. Connects to ArduPilot SITL via MAVLink
- ROS 2 Humble/Jazzy sourced 2. Gets telemetry (position, attitude, velocity)
- ~/ardu_ws workspace built and sourced 3. Runs your drone_controller.calculate_landing_maneuver()
4. Sends velocity commands to ArduPilot
""" """
import argparse
import os
import signal
import subprocess
import sys import sys
import time import time
from pathlib import Path import math
import argparse
try:
from pymavlink import mavutil
except ImportError:
print("ERROR: pymavlink not installed")
print("Run: pip install pymavlink")
sys.exit(1)
from config import ARDUPILOT, CONTROLLER, MAVLINK
from drone_controller import DroneController
def check_ros2(): class ArduPilotInterface:
"""Check if ROS 2 is available.""" """Interface to ArduPilot SITL via MAVLink."""
try:
subprocess.run(['ros2', '--help'], capture_output=True, check=True)
return True
except (subprocess.CalledProcessError, FileNotFoundError):
return False
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): def __init__(self, connection_string=None):
# Update environment by sourcing the workspace self.connection_string = connection_string or MAVLINK["connection_string"]
# This is done by running commands in a sourced shell self.mav = None
return True self.armed = False
return False self.mode = "UNKNOWN"
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 # Telemetry state
'rover': ('ardupilot_gz_bringup', 'wildthumper_playpen.launch.py'), self.position = {"x": 0, "y": 0, "z": 0}
'wildthumper': ('ardupilot_gz_bringup', 'wildthumper_playpen.launch.py'), self.velocity = {"x": 0, "y": 0, "z": 0}
self.attitude = {"roll": 0, "pitch": 0, "yaw": 0}
self.altitude = 0
self.vertical_velocity = 0
def connect(self, timeout=30):
"""Connect to ArduPilot SITL."""
print(f"[INFO] Connecting to {self.connection_string}...")
# 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 term_cmd in terminals:
try: try:
subprocess.Popen(term_cmd) self.mav = mavutil.mavlink_connection(self.connection_string)
self.mav.wait_heartbeat(timeout=timeout)
print(f"[OK] Connected to system {self.mav.target_system}")
return True return True
except FileNotFoundError: except Exception as e:
continue print(f"[ERROR] Connection failed: {e}")
return False
print(f"[WARN] Could not open terminal for MAVProxy") def update_telemetry(self):
print(f"[INFO] Run manually: {mavproxy_cmd}") """Read and update telemetry from ArduPilot."""
return False # Read all available messages
while True:
msg = self.mav.recv_match(blocking=False)
def parse_args(): if msg is None:
parser = argparse.ArgumentParser( break
description='Launch ArduPilot SITL with ROS 2 and Gazebo',
formatter_class=argparse.RawDescriptionHelpFormatter, msg_type = msg.get_type()
epilog="""
Examples: if msg_type == "HEARTBEAT":
python run_ardupilot.py # Iris on runway self.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
python run_ardupilot.py --world maze # Iris in maze # Get mode name
python run_ardupilot.py --vehicle rover # WildThumper rover if hasattr(mavutil.mavlink, 'enums'):
python run_ardupilot.py --mavproxy # With MAVProxy mode_map = mavutil.mavlink.enums.get('COPTER_MODE', {})
self.mode = mode_map.get(msg.custom_mode, {}).get('name', str(msg.custom_mode))
Available worlds: runway, maze, sitl (no Gazebo)
Available vehicles: copter, rover elif msg_type == "LOCAL_POSITION_NED":
self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
self.velocity = {"x": msg.vx, "y": msg.vy, "z": msg.vz}
self.altitude = -msg.z # NED: down is positive
self.vertical_velocity = -msg.vz
elif msg_type == "ATTITUDE":
self.attitude = {
"roll": msg.roll,
"pitch": msg.pitch,
"yaw": msg.yaw
}
def get_telemetry(self):
"""Get telemetry in the same format as standalone simulation."""
self.update_telemetry()
return {
"imu": {
"orientation": self.attitude,
"angular_velocity": {"x": 0, "y": 0, "z": 0} # Not available via MAVLink easily
},
"altimeter": {
"altitude": self.altitude,
"vertical_velocity": self.vertical_velocity
},
"velocity": self.velocity,
"landing_pad": None, # Not available in ArduPilot mode - use vision
"position": self.position, # Extra: actual position (GPS-like)
"armed": self.armed,
"mode": self.mode
}
def set_mode(self, mode_name):
"""Set flight mode."""
mode_map = self.mav.mode_mapping()
if mode_name.upper() not in mode_map:
print(f"[WARN] Unknown mode: {mode_name}")
return False
mode_id = mode_map[mode_name.upper()]
self.mav.set_mode(mode_id)
print(f"[OK] Mode set to {mode_name}")
return True
def arm(self, force=True):
"""Arm motors."""
if force:
# Force arm (bypass checks)
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0, 1, 21196, 0, 0, 0, 0, 0 # 21196 = force arm
)
else:
self.mav.arducopter_arm()
time.sleep(1)
self.update_telemetry()
if self.armed:
print("[OK] Armed")
return self.armed
def disarm(self):
"""Disarm motors."""
self.mav.arducopter_disarm()
print("[OK] Disarmed")
def takeoff(self, altitude=5.0):
"""Takeoff to specified altitude."""
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
0, 0, 0, 0, 0, 0, 0, altitude
)
print(f"[OK] Takeoff to {altitude}m")
def send_velocity(self, vx, vy, vz, yaw_rate=0):
"""Send velocity command in body frame.
Args:
vx: Forward velocity (m/s)
vy: Right velocity (m/s)
vz: Down velocity (m/s, positive = descend)
yaw_rate: Yaw rate (rad/s)
""" """
) # Type mask: use velocity only
type_mask = (
parser.add_argument( mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
'--world', '-w', type=str, default='runway', mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
choices=['runway', 'maze', 'sitl'], mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
help='Simulation world (default: runway)' mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
) mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
parser.add_argument( mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
'--vehicle', '-v', type=str, default='copter', mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE
choices=['copter', 'rover'], )
help='Vehicle type (default: copter)'
) self.mav.mav.set_position_target_local_ned_send(
parser.add_argument( 0, # time_boot_ms
'--mavproxy', '-m', action='store_true', self.mav.target_system,
help='Also launch MAVProxy in a new terminal' self.mav.target_component,
) mavutil.mavlink.MAV_FRAME_BODY_NED,
parser.add_argument( type_mask,
'--mavproxy-port', type=int, default=14550, 0, 0, 0, # position (ignored)
help='MAVProxy master port (default: 14550)' vx, vy, vz, # velocity
) 0, 0, 0, # acceleration (ignored)
0, yaw_rate # yaw, yaw_rate
return parser.parse_args() )
def main(): def main():
args = parse_args() parser = argparse.ArgumentParser(description="ArduPilot Drone Controller")
parser.add_argument("--connection", "-c", default=None,
help="MAVLink connection string (default: from config.py)")
parser.add_argument("--takeoff-alt", "-a", type=float, default=5.0,
help="Takeoff altitude (meters)")
parser.add_argument("--no-takeoff", action="store_true",
help="Don't auto takeoff, just control")
args = parser.parse_args()
print("=" * 60) # Create ArduPilot interface
print(" ArduPilot SITL + Gazebo (Official ROS 2 DDS)") ardupilot = ArduPilotInterface(args.connection)
print("=" * 60)
print()
# Check ROS 2 # Connect
if not check_ros2(): if not ardupilot.connect():
print("[ERROR] ROS 2 not found!") print("[ERROR] Could not connect to ArduPilot")
print("Please source ROS 2:") print("Make sure sim_vehicle.py is running")
print(" source /opt/ros/humble/setup.bash") sys.exit(1)
return 1
print("[OK] ROS 2 available") # Create drone controller
controller = DroneController()
# Check ArduPilot packages print("\n" + "=" * 50)
if not check_ardupilot_packages(): print(" ArduPilot GPS-Denied Controller")
print("[ERROR] ArduPilot ROS 2 packages not found!") print("=" * 50)
print() print(f"Mode: {ardupilot.mode}")
print("Please install ArduPilot ROS 2:") print(f"Armed: {ardupilot.armed}")
print(" ./setup/install_ardupilot.sh") print("")
print()
print("Then source the workspace:")
print(" source ~/ardu_ws/install/setup.bash")
return 1
print("[OK] ArduPilot ROS 2 packages found") if not args.no_takeoff:
# Set GUIDED mode and arm
print("[INFO] Setting up for takeoff...")
ardupilot.set_mode("GUIDED")
time.sleep(1)
ardupilot.arm(force=True)
time.sleep(1)
if ardupilot.armed:
ardupilot.takeoff(args.takeoff_alt)
print(f"[INFO] Taking off to {args.takeoff_alt}m...")
# Wait for takeoff
for _ in range(50): # 5 seconds
telemetry = ardupilot.get_telemetry()
if telemetry["altimeter"]["altitude"] > args.takeoff_alt * 0.9:
print("[OK] Reached target altitude")
break
time.sleep(0.1)
# Get launch command print("\n[INFO] Starting controller loop...")
launch_cmd = get_launch_command(args.world, args.vehicle) print("[INFO] Press Ctrl+C to stop\n")
print() rate = 20 # Hz
print(f"World: {args.world}")
print(f"Vehicle: {args.vehicle}")
print(f"Launch: {' '.join(launch_cmd)}")
print()
# Launch MAVProxy if requested
if args.mavproxy:
print("[INFO] Starting MAVProxy...")
# Delay to let SITL start first
time.sleep(2)
launch_mavproxy(args.mavproxy_port)
print("Starting simulation...")
print("Press Ctrl+C to stop.")
print()
print("-" * 60)
# Handle Ctrl+C gracefully
def signal_handler(sig, frame):
print("\nShutting down...")
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
# Run the launch command
try: try:
subprocess.run(launch_cmd, check=True) while True:
except subprocess.CalledProcessError as e: # Get telemetry
print(f"[ERROR] Launch failed: {e}") telemetry = ardupilot.get_telemetry()
return 1
# Run your controller
thrust, pitch, roll, yaw = controller.calculate_landing_maneuver(
telemetry,
rover_telemetry=None # No rover in ArduPilot mode yet
)
# Convert control outputs to velocities
# thrust -> vertical velocity (negative = up)
# pitch -> forward velocity
# roll -> right velocity
vz = -thrust * 2.0 # Scale factor
vx = pitch * 2.0
vy = roll * 2.0
yaw_rate = yaw * 1.0
# Send velocity command
ardupilot.send_velocity(vx, vy, vz, yaw_rate)
# Print status
alt = telemetry["altimeter"]["altitude"]
mode = telemetry.get("mode", "?")
print(f"\rAlt: {alt:5.1f}m | Mode: {mode:10} | Cmd: vx={vx:+.2f} vy={vy:+.2f} vz={vz:+.2f}", end="")
time.sleep(1.0 / rate)
except KeyboardInterrupt: except KeyboardInterrupt:
print("\nShutdown complete.") print("\n\n[INFO] Stopping...")
ardupilot.set_mode("LAND")
return 0 print("[OK] Landing mode set")
if __name__ == '__main__': if __name__ == "__main__":
sys.exit(main()) main()

61
scripts/run_ardupilot_controller.sh Executable file
View File

@@ -0,0 +1,61 @@
#!/bin/bash
# =============================================================================
# ArduPilot Controller Launcher
# =============================================================================
# Starts ArduPilot SITL and your drone controller together.
#
# Usage:
# Terminal 1: ./scripts/run_ardupilot_sim.sh runway
# Terminal 2: ./scripts/run_ardupilot_controller.sh
# =============================================================================
set -e
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
echo "=============================================="
echo " ArduPilot Controller"
echo "=============================================="
echo ""
# Check dependencies
if ! command -v sim_vehicle.py &> /dev/null; then
echo "[ERROR] sim_vehicle.py not found"
echo "Add to PATH: export PATH=\$PATH:~/ardupilot/Tools/autotest"
exit 1
fi
# Activate virtual environment if exists
if [ -f "$PROJECT_DIR/venv/bin/activate" ]; then
source "$PROJECT_DIR/venv/bin/activate"
fi
# Start SITL in background
echo "[INFO] Starting ArduPilot SITL..."
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy &
SITL_PID=$!
# Wait for SITL to start
sleep 5
# Check if SITL started
if ! kill -0 $SITL_PID 2>/dev/null; then
echo "[ERROR] SITL failed to start"
exit 1
fi
echo "[OK] SITL started (PID: $SITL_PID)"
echo ""
# Trap to kill SITL on exit
trap "echo ''; echo '[INFO] Stopping SITL...'; kill $SITL_PID 2>/dev/null; exit 0" INT TERM
# Run controller
echo "[INFO] Starting drone controller..."
echo ""
cd "$PROJECT_DIR"
python run_ardupilot.py "$@"
# Cleanup
kill $SITL_PID 2>/dev/null