139 lines
4.4 KiB
Python
139 lines
4.4 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
ArduPilot ROS 2 Launch Helper
|
|
|
|
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.
|
|
|
|
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, LogInfo
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
|
|
|
|
def generate_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', 'drone_landing.sdf')
|
|
|
|
# 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'):
|
|
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]
|
|
|
|
actions = [
|
|
DeclareLaunchArgument(
|
|
'use_sim_time',
|
|
default_value='true',
|
|
description='Use simulation clock'
|
|
),
|
|
]
|
|
|
|
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',
|
|
),
|
|
|
|
# ROS-Gazebo Bridge
|
|
TimerAction(
|
|
period=2.0,
|
|
actions=[
|
|
Node(
|
|
package='ros_gz_bridge',
|
|
executable='parameter_bridge',
|
|
name='gz_bridge',
|
|
arguments=[
|
|
# Rover velocity commands
|
|
'/rover/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
|
|
# Drone velocity commands
|
|
'/drone/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
|
|
# Odometry
|
|
'/model/drone/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
|
|
'/model/rover/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
|
|
# 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 ROS 2 Launch Helper")
|
|
print("=" * 60)
|
|
print()
|
|
print("RECOMMENDED: Use official ArduPilot ROS 2 packages:")
|
|
print()
|
|
print(" # Install ArduPilot ROS 2")
|
|
print(" ./setup/install_ardupilot.sh")
|
|
print()
|
|
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()
|