88 lines
2.5 KiB
Python
88 lines
2.5 KiB
Python
|
from ament_index_python.packages import get_package_share_directory
|
||
|
|
import os
|
||
|
|
from clearpath_config.common.utils.yaml import read_yaml
|
||
|
|
from clearpath_config.clearpath_config import ClearpathConfig
|
||
|
|
|
||
|
|
from launch import LaunchDescription
|
||
|
|
from launch.actions import (
|
||
|
|
DeclareLaunchArgument,
|
||
|
|
GroupAction,
|
||
|
|
IncludeLaunchDescription,
|
||
|
|
OpaqueFunction
|
||
|
|
)
|
||
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||
|
|
|
||
|
|
from launch.substitutions import (
|
||
|
|
LaunchConfiguration,
|
||
|
|
PathJoinSubstitution
|
||
|
|
)
|
||
|
|
|
||
|
|
from launch_ros.actions import PushRosNamespace
|
||
|
|
|
||
|
|
|
||
|
|
ARGUMENTS = [
|
||
|
|
DeclareLaunchArgument('use_sim_time', default_value='false',
|
||
|
|
choices=['true', 'false'],
|
||
|
|
description='Use sim time'),
|
||
|
|
DeclareLaunchArgument('setup_path',
|
||
|
|
default_value='/etc/clearpath/',
|
||
|
|
description='Clearpath setup path')
|
||
|
|
]
|
||
|
|
|
||
|
|
|
||
|
|
def launch_setup(context, *args, **kwargs):
|
||
|
|
# Packages
|
||
|
|
pkg_nav2_bringup = get_package_share_directory('nav2_bringup')
|
||
|
|
package_config_path = get_package_share_directory('locker98_tools_bringup')
|
||
|
|
|
||
|
|
# Launch Configurations
|
||
|
|
use_sim_time = LaunchConfiguration('use_sim_time')
|
||
|
|
setup_path = LaunchConfiguration('setup_path')
|
||
|
|
map = LaunchConfiguration('map')
|
||
|
|
|
||
|
|
# Read robot YAML
|
||
|
|
config = read_yaml(setup_path.perform(context) + 'robot.yaml')
|
||
|
|
# Parse robot YAML into config
|
||
|
|
clearpath_config = ClearpathConfig(config)
|
||
|
|
|
||
|
|
namespace = clearpath_config.system.namespace
|
||
|
|
platform_model = clearpath_config.platform.get_platform_model()
|
||
|
|
|
||
|
|
file_parameters = os.path.join(
|
||
|
|
package_config_path,
|
||
|
|
'config',
|
||
|
|
platform_model,
|
||
|
|
'localization.yaml'
|
||
|
|
)
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
launch_localization = PathJoinSubstitution(
|
||
|
|
[pkg_nav2_bringup, 'launch', 'localization_launch.py'])
|
||
|
|
|
||
|
|
localization = GroupAction([
|
||
|
|
PushRosNamespace(namespace),
|
||
|
|
|
||
|
|
IncludeLaunchDescription(
|
||
|
|
PythonLaunchDescriptionSource(launch_localization),
|
||
|
|
launch_arguments=[
|
||
|
|
('namespace', namespace),
|
||
|
|
('map', map),
|
||
|
|
('use_sim_time', use_sim_time),
|
||
|
|
('params_file', file_parameters)
|
||
|
|
]
|
||
|
|
),
|
||
|
|
])
|
||
|
|
|
||
|
|
return [localization]
|
||
|
|
|
||
|
|
|
||
|
|
def generate_launch_description():
|
||
|
|
map_arg = DeclareLaunchArgument(
|
||
|
|
'map',
|
||
|
|
description='Full path to map yaml file to load')
|
||
|
|
|
||
|
|
ld = LaunchDescription(ARGUMENTS)
|
||
|
|
ld.add_action(map_arg)
|
||
|
|
ld.add_action(OpaqueFunction(function=launch_setup))
|
||
|
|
return ld
|