Initial Commit
This commit is contained in:
169
launch/full_simulation.launch.py
Normal file
169
launch/full_simulation.launch.py
Normal file
@@ -0,0 +1,169 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Full Simulation Launch - GPS-Denied Navigation."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction, IncludeLaunchDescription
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.conditions import IfCondition
|
||||
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'
|
||||
)
|
||||
|
||||
use_nvidia_arg = DeclareLaunchArgument(
|
||||
'use_nvidia', default_value='true', description='Enable NVIDIA GPU'
|
||||
)
|
||||
|
||||
headless_arg = DeclareLaunchArgument(
|
||||
'headless', default_value='false', description='Run without GUI'
|
||||
)
|
||||
|
||||
use_ground_truth_arg = DeclareLaunchArgument(
|
||||
'use_ground_truth', default_value='true', description='Use Gazebo ground truth'
|
||||
)
|
||||
|
||||
gazebo = ExecuteProcess(
|
||||
cmd=['gazebo', '--verbose', LaunchConfiguration('world')],
|
||||
output='screen',
|
||||
additional_env={
|
||||
'GAZEBO_MODEL_PATH': f"{pkg_share}/models:" + os.path.expanduser('~/ardupilot_gazebo/models'),
|
||||
'GAZEBO_RESOURCE_PATH': f"{pkg_share}/worlds:" + os.path.expanduser('~/ardupilot_gazebo/worlds')
|
||||
}
|
||||
)
|
||||
|
||||
ardupilot_uav = TimerAction(
|
||||
period=3.0,
|
||||
actions=[
|
||||
ExecuteProcess(
|
||||
cmd=[
|
||||
'sim_vehicle.py',
|
||||
'-v', 'ArduCopter',
|
||||
'-f', 'gazebo-iris',
|
||||
'--model', 'JSON',
|
||||
'--map', '--console',
|
||||
'-I0',
|
||||
'--out', '127.0.0.1:14550',
|
||||
'--out', '127.0.0.1:14551',
|
||||
'--add-param-file', os.path.join(pkg_share, 'config', 'ardupilot_gps_denied.parm')
|
||||
],
|
||||
cwd=os.path.expanduser('~/ardupilot/ArduCopter'),
|
||||
output='screen'
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
mavros_uav = TimerAction(
|
||||
period=8.0,
|
||||
actions=[
|
||||
Node(
|
||||
package='mavros',
|
||||
executable='mavros_node',
|
||||
name='mavros_uav',
|
||||
namespace='uav',
|
||||
output='screen',
|
||||
parameters=[
|
||||
os.path.join(pkg_share, 'config', 'mavros_params.yaml'),
|
||||
{
|
||||
'fcu_url': 'udp://:14550@127.0.0.1:14555',
|
||||
'gcs_url': '',
|
||||
'target_system_id': 1,
|
||||
'target_component_id': 1,
|
||||
'fcu_protocol': 'v2.0'
|
||||
}
|
||||
]
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
visual_odom_node = 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')]
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
geofence_node = TimerAction(
|
||||
period=12.0,
|
||||
actions=[
|
||||
Node(
|
||||
package='uav_ugv_simulation',
|
||||
executable='geofence_node',
|
||||
name='geofence_monitor',
|
||||
namespace='uav',
|
||||
output='screen',
|
||||
parameters=[os.path.join(pkg_share, 'config', 'geofence_params.yaml')]
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
vision_nav_node = TimerAction(
|
||||
period=14.0,
|
||||
actions=[
|
||||
Node(
|
||||
package='uav_ugv_simulation',
|
||||
executable='vision_nav_node',
|
||||
name='vision_nav_node',
|
||||
namespace='uav',
|
||||
output='screen',
|
||||
parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')]
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
uav_controller = TimerAction(
|
||||
period=15.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')]
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
failsafe_handler = TimerAction(
|
||||
period=16.0,
|
||||
actions=[
|
||||
Node(
|
||||
package='uav_ugv_simulation',
|
||||
executable='failsafe_handler',
|
||||
name='failsafe_handler',
|
||||
output='screen',
|
||||
parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')]
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
world_arg,
|
||||
use_nvidia_arg,
|
||||
headless_arg,
|
||||
use_ground_truth_arg,
|
||||
gazebo,
|
||||
ardupilot_uav,
|
||||
mavros_uav,
|
||||
visual_odom_node,
|
||||
geofence_node,
|
||||
vision_nav_node,
|
||||
uav_controller,
|
||||
failsafe_handler,
|
||||
])
|
||||
97
launch/uav_only.launch.py
Normal file
97
launch/uav_only.launch.py
Normal file
@@ -0,0 +1,97 @@
|
||||
#!/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=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:" + os.path.expanduser('~/ardupilot_gazebo/models'),
|
||||
}
|
||||
)
|
||||
|
||||
ardupilot = TimerAction(
|
||||
period=3.0,
|
||||
actions=[
|
||||
ExecuteProcess(
|
||||
cmd=[
|
||||
'sim_vehicle.py', '-v', 'ArduCopter', '-f', 'gazebo-iris',
|
||||
'--model', 'JSON', '--map', '--console', '-I0',
|
||||
'--out', '127.0.0.1:14550',
|
||||
'--add-param-file', os.path.join(pkg_share, 'config', 'ardupilot_gps_denied.parm')
|
||||
],
|
||||
cwd=os.path.expanduser('~/ardupilot/ArduCopter'),
|
||||
output='screen'
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
mavros = TimerAction(
|
||||
period=8.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': 'udp://:14550@127.0.0.1:14555'}
|
||||
]
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
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,
|
||||
])
|
||||
63
launch/ugv_only.launch.py
Normal file
63
launch/ugv_only.launch.py
Normal file
@@ -0,0 +1,63 @@
|
||||
#!/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,
|
||||
])
|
||||
21
launch/utils/launch_helpers.py
Normal file
21
launch/utils/launch_helpers.py
Normal file
@@ -0,0 +1,21 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Launch file helper utilities."""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def get_package_share_path(package_name):
|
||||
return get_package_share_directory(package_name)
|
||||
|
||||
|
||||
def get_config_path(package_name, config_file):
|
||||
return os.path.join(get_package_share_path(package_name), 'config', config_file)
|
||||
|
||||
|
||||
def get_world_path(package_name, world_file):
|
||||
return os.path.join(get_package_share_path(package_name), 'worlds', world_file)
|
||||
|
||||
|
||||
def get_model_path(package_name, model_name):
|
||||
return os.path.join(get_package_share_path(package_name), 'models', model_name)
|
||||
Reference in New Issue
Block a user