104 lines
3.0 KiB
Python
104 lines
3.0 KiB
Python
#!/usr/bin/env python3
|
|
"""UAV-only Simulation Launch."""
|
|
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
import os
|
|
|
|
|
|
def generate_launch_description():
|
|
pkg_share = FindPackageShare('uav_ugv_simulation').find('uav_ugv_simulation')
|
|
|
|
world_arg = DeclareLaunchArgument(
|
|
'world',
|
|
default_value='iris_runway.sdf',
|
|
description='World file name'
|
|
)
|
|
|
|
# Gazebo Harmonic
|
|
gazebo = ExecuteProcess(
|
|
cmd=[
|
|
'gz', 'sim', '-v4', '-r',
|
|
os.path.expanduser('~/ardupilot_gazebo/worlds/iris_runway.sdf')
|
|
],
|
|
output='screen',
|
|
additional_env={
|
|
'GZ_SIM_RESOURCE_PATH': f"{pkg_share}/models:{os.path.expanduser('~/ardupilot_gazebo/models')}:{os.path.expanduser('~/ardupilot_gazebo/worlds')}",
|
|
'GZ_SIM_SYSTEM_PLUGIN_PATH': os.path.expanduser('~/ardupilot_gazebo/build')
|
|
}
|
|
)
|
|
|
|
ardupilot = TimerAction(
|
|
period=5.0,
|
|
actions=[
|
|
ExecuteProcess(
|
|
cmd=[
|
|
'sim_vehicle.py', '-v', 'ArduCopter', '-f', 'gazebo-iris',
|
|
'--model', 'JSON', '--no-mavproxy', '-I0',
|
|
],
|
|
cwd=os.path.expanduser('~/ardupilot'),
|
|
output='screen',
|
|
additional_env={
|
|
'PATH': os.environ.get('PATH', '') + ':' + os.path.expanduser('~/ardupilot/Tools/autotest')
|
|
}
|
|
)
|
|
]
|
|
)
|
|
|
|
mavros = TimerAction(
|
|
period=15.0,
|
|
actions=[
|
|
Node(
|
|
package='mavros',
|
|
executable='mavros_node',
|
|
name='mavros',
|
|
namespace='uav',
|
|
output='screen',
|
|
parameters=[
|
|
os.path.join(pkg_share, 'config', 'mavros_params.yaml'),
|
|
{'fcu_url': 'tcp://127.0.0.1:5760'}
|
|
]
|
|
)
|
|
]
|
|
)
|
|
|
|
visual_odom = TimerAction(
|
|
period=10.0,
|
|
actions=[
|
|
Node(
|
|
package='uav_ugv_simulation',
|
|
executable='visual_odom_node',
|
|
name='visual_odom_node',
|
|
namespace='uav',
|
|
output='screen',
|
|
parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')]
|
|
)
|
|
]
|
|
)
|
|
|
|
uav_controller = TimerAction(
|
|
period=12.0,
|
|
actions=[
|
|
Node(
|
|
package='uav_ugv_simulation',
|
|
executable='uav_controller',
|
|
name='uav_controller',
|
|
namespace='uav',
|
|
output='screen',
|
|
parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')]
|
|
)
|
|
]
|
|
)
|
|
|
|
return LaunchDescription([
|
|
world_arg,
|
|
gazebo,
|
|
ardupilot,
|
|
mavros,
|
|
visual_odom,
|
|
uav_controller,
|
|
])
|