ArduPilot SITL Update
This commit is contained in:
472
run_ardupilot.py
472
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
|
||||
|
||||
|
||||
Reference in New Issue
Block a user