64 lines
1.8 KiB
Python
64 lines
1.8 KiB
Python
#!/usr/bin/env python3
|
|
"""UGV-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=os.path.join(pkg_share, 'worlds', 'empty_custom.world'),
|
|
description='Path to world file'
|
|
)
|
|
|
|
gazebo = ExecuteProcess(
|
|
cmd=['gazebo', '--verbose', LaunchConfiguration('world')],
|
|
output='screen',
|
|
additional_env={
|
|
'GAZEBO_MODEL_PATH': f"{pkg_share}/models",
|
|
}
|
|
)
|
|
|
|
ugv_spawn = TimerAction(
|
|
period=3.0,
|
|
actions=[
|
|
ExecuteProcess(
|
|
cmd=[
|
|
'ros2', 'run', 'gazebo_ros', 'spawn_entity.py',
|
|
'-entity', 'ugv',
|
|
'-file', os.path.join(pkg_share, 'models', 'custom_ugv', 'model.sdf'),
|
|
'-x', '5.0', '-y', '0.0', '-z', '0.1'
|
|
],
|
|
output='screen'
|
|
)
|
|
]
|
|
)
|
|
|
|
ugv_controller = TimerAction(
|
|
period=5.0,
|
|
actions=[
|
|
Node(
|
|
package='uav_ugv_simulation',
|
|
executable='ugv_controller',
|
|
name='ugv_controller',
|
|
namespace='ugv',
|
|
output='screen',
|
|
parameters=[os.path.join(pkg_share, 'config', 'ugv_params.yaml')]
|
|
)
|
|
]
|
|
)
|
|
|
|
return LaunchDescription([
|
|
world_arg,
|
|
gazebo,
|
|
ugv_spawn,
|
|
ugv_controller,
|
|
])
|