86 lines
2.6 KiB
Python
86 lines
2.6 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, SetRemap
|
||
|
|
|
||
|
|
|
||
|
|
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')
|
||
|
|
|
||
|
|
# 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,
|
||
|
|
'nav2.yaml'
|
||
|
|
)
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
launch_nav2 = PathJoinSubstitution(
|
||
|
|
[pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
|
||
|
|
|
||
|
|
nav2 = GroupAction([
|
||
|
|
PushRosNamespace(namespace),
|
||
|
|
SetRemap('/' + namespace + '/global_costmap/sensors/lidar3d_0/scan',
|
||
|
|
'/' + namespace + '/sensors/lidar3d_0/scan'),
|
||
|
|
SetRemap('/' + namespace + '/local_costmap/sensors/lidar3d_0/scan',
|
||
|
|
'/' + namespace + '/sensors/lidar3d_0/scan'),
|
||
|
|
|
||
|
|
IncludeLaunchDescription(
|
||
|
|
PythonLaunchDescriptionSource(launch_nav2),
|
||
|
|
launch_arguments=[
|
||
|
|
('use_sim_time', use_sim_time),
|
||
|
|
('params_file', file_parameters),
|
||
|
|
('use_composition', 'False'),
|
||
|
|
('namespace', namespace)
|
||
|
|
]
|
||
|
|
),
|
||
|
|
])
|
||
|
|
|
||
|
|
return [nav2]
|
||
|
|
|
||
|
|
|
||
|
|
def generate_launch_description():
|
||
|
|
ld = LaunchDescription(ARGUMENTS)
|
||
|
|
ld.add_action(OpaqueFunction(function=launch_setup))
|
||
|
|
return ld
|