135 lines
4.5 KiB
Python
135 lines
4.5 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
ArduPilot SITL + Gazebo Launch File
|
|
Works with Gazebo Harmonic/Garden and ArduPilot SITL.
|
|
|
|
This launch file:
|
|
1. Starts Gazebo with ArduPilot-compatible drone model
|
|
2. Sets up ROS-Gazebo bridge for telemetry
|
|
3. Optionally starts ArduPilot SITL
|
|
|
|
NOTE: ArduPilot SITL integration requires the ardupilot_gazebo plugin.
|
|
Install from: https://github.com/ArduPilot/ardupilot_gazebo
|
|
"""
|
|
|
|
import os
|
|
import shutil
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
|
|
|
|
def generate_launch_description():
|
|
"""Generate the launch description."""
|
|
|
|
# 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')
|
|
|
|
# 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)
|
|
|
|
# Determine Gazebo command
|
|
if shutil.which('gz'):
|
|
gz_cmd = ['gz', 'sim', '-r', world_file]
|
|
elif shutil.which('ign'):
|
|
gz_cmd = ['ign', 'gazebo', '-r', world_file]
|
|
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
|
|
ExecuteProcess(
|
|
cmd=gz_cmd,
|
|
output='screen',
|
|
additional_env=env
|
|
),
|
|
|
|
# ROS-Gazebo Bridge for telemetry and commands
|
|
# Delayed start to wait for Gazebo
|
|
TimerAction(
|
|
period=2.0,
|
|
actions=[
|
|
Node(
|
|
package='ros_gz_bridge',
|
|
executable='parameter_bridge',
|
|
name='gz_bridge',
|
|
arguments=[
|
|
# Rover velocity commands (ROS to Gazebo)
|
|
'/rover/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
|
|
# Odometry (from Gazebo to ROS) - fallback if MAVLink not used
|
|
'/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',
|
|
# Clock
|
|
'/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
|
|
],
|
|
parameters=[{'use_sim_time': use_sim_time}],
|
|
output='screen'
|
|
),
|
|
]
|
|
),
|
|
]
|
|
|
|
return LaunchDescription(actions)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
print("=" * 60)
|
|
print(" ArduPilot SITL + Gazebo Launch File")
|
|
print("=" * 60)
|
|
print()
|
|
print("This is a ROS 2 launch file for ArduPilot integration.")
|
|
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()
|
|
print("Usage:")
|
|
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")
|