Update the SLAM launch file so it works with the latest version of sl…

…am_toolbox. Rather than re-impementing all of the launch file, just include the online_sync or online_async launch files as appropriate

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