diff --git a/turtlebot4_navigation/launch/slam.launch.py b/turtlebot4_navigation/launch/slam.launch.py index 918f3ec..5c1323c 100644 --- a/turtlebot4_navigation/launch/slam.launch.py +++ b/turtlebot4_navigation/launch/slam.launch.py @@ -18,14 +18,22 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + OpaqueFunction +) from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node, PushRosNamespace +from launch_ros.actions import Node, PushRosNamespace, SetRemap from nav2_common.launch import RewrittenYaml +pkg_turtlebot4_navigation = get_package_share_directory('turtlebot4_navigation') +pkg_slam_toolbox = get_package_share_directory('slam_toolbox') ARGUMENTS = [ DeclareLaunchArgument('use_sim_time', default_value='false', @@ -35,64 +43,72 @@ choices=['true', 'false'], description='Use synchronous SLAM'), DeclareLaunchArgument('namespace', default_value='', - description='Robot namespace') + description='Robot namespace'), + DeclareLaunchArgument('autostart', default_value='true', + choices=['true', 'false'], + description='Automatically startup the slamtoolbox. Ignored when use_lifecycle_manager is true.'), + DeclareLaunchArgument('use_lifecycle_manager', default_value='false', + choices=['true', 'false'], + description='Enable bond connection during node activation'), + DeclareLaunchArgument('params', + default_value=PathJoinSubstitution([pkg_turtlebot4_navigation, 'config', 'slam.yaml']), + description='Path to the SLAM Toolbox configuration file') ] -def generate_launch_description(): - pkg_turtlebot4_navigation = get_package_share_directory('turtlebot4_navigation') - +def launch_setup(context, *args, **kwargs): namespace = LaunchConfiguration('namespace') sync = LaunchConfiguration('sync') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + use_lifecycle_manager = LaunchConfiguration('use_lifecycle_manager') + slam_params = LaunchConfiguration('params') - slam_params_arg = DeclareLaunchArgument( - 'params', - default_value=PathJoinSubstitution( - [pkg_turtlebot4_navigation, 'config', 'slam.yaml']), - description='Robot namespace') - - slam_params = RewrittenYaml( - source_file=LaunchConfiguration('params'), - root_key=namespace, - param_rewrites={}, - convert_types=True - ) - - remappings = [ - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - ('/scan', 'scan'), - ('/map', 'map'), - ('/map_metadata', 'map_metadata'), - ] + namespace_str = namespace.perform(context) + if (namespace_str and not namespace_str.startswith('/')): + namespace_str = '/' + namespace_str + + launch_slam_sync = PathJoinSubstitution( + [pkg_slam_toolbox, 'launch', 'online_sync_launch.py']) + + launch_slam_async = PathJoinSubstitution( + [pkg_slam_toolbox, 'launch', 'online_async_launch.py']) slam = GroupAction([ PushRosNamespace(namespace), - Node(package='slam_toolbox', - executable='sync_slam_toolbox_node', - name='slam_toolbox', - output='screen', - parameters=[ - slam_params, - {'use_sim_time': LaunchConfiguration('use_sim_time')} - ], - remappings=remappings, - condition=IfCondition(sync)), - - Node(package='slam_toolbox', - executable='async_slam_toolbox_node', - name='slam_toolbox', - output='screen', - parameters=[ - slam_params, - {'use_sim_time': LaunchConfiguration('use_sim_time')} - ], - remappings=remappings, - condition=UnlessCondition(sync)) + SetRemap('/tf', namespace_str + '/tf'), + SetRemap('/tf_static', namespace_str + '/tf_static'), + SetRemap('/scan', namespace_str + '/scan'), + SetRemap('/map', namespace_str + '/map'), + SetRemap('/map_metadata', namespace_str + '/map_metadata'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(launch_slam_sync), + launch_arguments = [ + ('use_sim_time', use_sim_time), + ('autostart', autostart), + ('use_lifecycle_manager', use_lifecycle_manager), + ('slam_params_file', slam_params) + ], + condition=IfCondition(sync) + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(launch_slam_async), + launch_arguments = [ + ('use_sim_time', use_sim_time), + ('autostart', autostart), + ('use_lifecycle_manager', use_lifecycle_manager), + ('slam_params_file', slam_params) + ], + condition=UnlessCondition(sync) + ) ]) + return [slam] + +def generate_launch_description(): ld = LaunchDescription(ARGUMENTS) - ld.add_action(slam_params_arg) - ld.add_action(slam) + ld.add_action(OpaqueFunction(function=launch_setup)) return ld