89 lines
2.3 KiB
Python
89 lines
2.3 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,
|
||
|
|
OpaqueFunction
|
||
|
|
)
|
||
|
|
|
||
|
|
from launch.substitutions import (
|
||
|
|
LaunchConfiguration,
|
||
|
|
PathJoinSubstitution
|
||
|
|
)
|
||
|
|
|
||
|
|
from launch_ros.actions import Node
|
||
|
|
|
||
|
|
from nav2_common.launch import RewrittenYaml
|
||
|
|
|
||
|
|
|
||
|
|
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
|
||
|
|
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,
|
||
|
|
'slam.yaml'
|
||
|
|
)
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
rewritten_parameters = RewrittenYaml(
|
||
|
|
source_file=file_parameters,
|
||
|
|
root_key=namespace,
|
||
|
|
param_rewrites={},
|
||
|
|
convert_types=True
|
||
|
|
)
|
||
|
|
|
||
|
|
slam = Node(
|
||
|
|
package='slam_toolbox',
|
||
|
|
executable='async_slam_toolbox_node',
|
||
|
|
name='slam_toolbox',
|
||
|
|
namespace=namespace,
|
||
|
|
output='screen',
|
||
|
|
parameters=[
|
||
|
|
rewritten_parameters,
|
||
|
|
{'use_sim_time': use_sim_time}
|
||
|
|
],
|
||
|
|
remappings=[
|
||
|
|
('/tf', 'tf'),
|
||
|
|
('/tf_static', 'tf_static'),
|
||
|
|
('/scan', 'sensors/lidar3d_0/scan'),
|
||
|
|
('/map', 'map'),
|
||
|
|
('/map_metadata', 'map_metadata'),
|
||
|
|
]
|
||
|
|
)
|
||
|
|
|
||
|
|
return [slam]
|
||
|
|
|
||
|
|
|
||
|
|
def generate_launch_description():
|
||
|
|
ld = LaunchDescription(ARGUMENTS)
|
||
|
|
ld.add_action(OpaqueFunction(function=launch_setup))
|
||
|
|
return ld
|