ROS2 and Gazebo: multiple robots

slam_launch.py

Figuring out how to Launch SLAM is a more challenging task than doing anything we did before, but MUCH less challenging then launching Nav2.

        
# Import required nodes

import os
...

def generate_launch_description():

    namespace = LaunchConfiguration('namespace')
    
    namespaced_params = LaunchConfiguration('namespaced_params')
    params_file = namespaced_params

    # Managed (by life cycle manager) node
    lifecycle_nodes = ['map_saver']

    # This file (at least for now) is outside our control. 
    # ROS2 has it, and (again, for now) we do not need to 
    # modify it, so we use it from ROS folder.
    # Note that there are two files, one for sync processing,
    # one for async processing. For our purpose, it doesn't mater.

    slam_toolbox_dir = get_package_share_directory('slam_toolbox')
    slam_launch_file = os.path.join(slam_toolbox_dir, 
        'launch', 'online_sync_launch.py')

    # YAML substitutions
    param_substitutions = {
        'use_sim_time': use_sim_time}

    configured_params = RewrittenYaml(
        source_file=namespaced_params,
        root_key=namespace,
        param_rewrites=param_substitutions,
        convert_types=True)

    # Declare the launch arguments
    ...

    # Note that "odd" use of 'params_file' persists here.
    # To be cleaned in future.
    declare_params_file_cmd = DeclareLaunchArgument(
        'params_file',
        default_value=def_nav2_params_path if namespace=='' else def_nav2_params_path_multi,
        description='Full path to the ROS2 parameters file to use for all launched nodes')

    ...

    # Using GroupAction and PushRosNamespace might be 
    # unnecessary, I'll revisit this code later.

    # Note that SLAN should (in a perfect world) be able to work
    # by just getting a namespace. But instead, it stumbles on
    # topics that it wants to be non-global. Just like that,
    # without any info in documentation. So I had to do some
    # remapping.

    start_slam_toolbox_cmd = GroupAction(
        [
            PushRosNamespace(namespace=namespace),

            SetRemap(src='/map', dst='map'),
            SetRemap(src='/map_metadata', dst='map_metadata'),
            SetRemap(src='/slam_toolbox/scan_visualization', dst='slam_toolbox/scan_visualization'),
            SetRemap(src='/slam_toolbox/graph_visualization', dst='slam_toolbox/graph_visualization'),
            
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(slam_launch_file),
                launch_arguments={
                    'use_sim_time': use_sim_time,
                    'slam_params_file': configured_params,
                }.items())
        ]
    )

    # Again, notice how nice native ROS2 nodes handle namespaces.
    # I don't even have to add a "name" tag!

    start_map_saver_server_cmd = Node(
        namespace=namespace,
        package='nav2_map_server',
        executable='map_saver_server',
        output='screen',
        emulate_tty=True,
        parameters=[configured_params],
    )  
    
    ...

    ld = LaunchDescription()

    ...

    ld.add_action(start_slam_toolbox_cmd)

    ld.add_action(start_map_saver_server_cmd)
    ld.add_action(start_lifecycle_manager_cmd)

    return ld

                    
                

(C) snowcron.com, all rights reserved

Please read the disclaimer