added amcl node

launch/robot_amcl.launch.py

@@ -17,15 +17,12 @@ import os
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction
-from launch.actions import SetEnvironmentVariable
+from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
 from launch.conditions import IfCondition
-from launch.substitutions import EqualsSubstitution
 from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch.substitutions import NotEqualsSubstitution
-from launch_ros.actions import LoadComposableNodes, SetParameter
+from launch_ros.actions import LoadComposableNodes
 from launch_ros.actions import Node
-from launch_ros.descriptions import ComposableNode, ParameterFile
+from launch_ros.descriptions import ComposableNode
 from nav2_common.launch import RewrittenYaml
 
 
@@ -55,13 +52,16 @@ def generate_launch_description():
     remappings = [('/tf', 'tf'),
                   ('/tf_static', 'tf_static')]
 
-    configured_params = ParameterFile(
-        RewrittenYaml(
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {
+        'use_sim_time': use_sim_time,
+        'yaml_filename': map_yaml_file}
+
+    configured_params = RewrittenYaml(
         source_file=params_file,
         root_key=namespace,
-            param_rewrites={},
-            convert_types=True),
-        allow_substs=True)
+        param_rewrites=param_substitutions,
+        convert_types=True)
 
     stdout_linebuf_envvar = SetEnvironmentVariable(
         'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
@@ -73,7 +73,6 @@ def generate_launch_description():
 
     declare_map_yaml_cmd = DeclareLaunchArgument(
         'map',
-        default_value='',
         description='Full path to map yaml file to load')
 
     declare_use_sim_time_cmd = DeclareLaunchArgument(
@@ -109,9 +108,7 @@ def generate_launch_description():
     load_nodes = GroupAction(
         condition=IfCondition(PythonExpression(['not ', use_composition])),
         actions=[
-            SetParameter('use_sim_time', use_sim_time),
             Node(
-                condition=IfCondition(EqualsSubstitution(LaunchConfiguration('map'), '')),
                 package='nav2_map_server',
                 executable='map_server',
                 name='map_server',
@@ -121,18 +118,6 @@ def generate_launch_description():
                 parameters=[configured_params],
                 arguments=['--ros-args', '--log-level', log_level],
                 remappings=remappings),
-            Node(
-                condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('map'), '')),
-                package='nav2_map_server',
-                executable='map_server',
-                name='map_server',
-                output='screen',
-                respawn=use_respawn,
-                respawn_delay=2.0,
-                parameters=[configured_params,
-                            {'yaml_filename': map_yaml_file}],
-                arguments=['--ros-args', '--log-level', log_level],
-                remappings=remappings),
             Node(
                 package='nav2_amcl',
                 executable='amcl',
@@ -149,22 +134,15 @@ def generate_launch_description():
                 name='lifecycle_manager_localization',
                 output='screen',
                 arguments=['--ros-args', '--log-level', log_level],
-                parameters=[{'autostart': autostart},
+                parameters=[{'use_sim_time': use_sim_time},
+                            {'autostart': autostart},
                             {'node_names': lifecycle_nodes}])
         ]
     )
-    # LoadComposableNode for map server twice depending if we should use the
-    # value of map from a CLI or launch default or user defined value in the
-    # yaml configuration file. They are separated since the conditions
-    # currently only work on the LoadComposableNodes commands and not on the
-    # ComposableNode node function itself
-    load_composable_nodes = GroupAction(
+
+    load_composable_nodes = LoadComposableNodes(
         condition=IfCondition(use_composition),
-        actions=[
-            SetParameter('use_sim_time', use_sim_time),
-            LoadComposableNodes(
         target_container=container_name_full,
-                condition=IfCondition(EqualsSubstitution(LaunchConfiguration('map'), '')),
         composable_node_descriptions=[
             ComposableNode(
                 package='nav2_map_server',
@@ -172,24 +150,6 @@ def generate_launch_description():
                 name='map_server',
                 parameters=[configured_params],
                 remappings=remappings),
-                ],
-            ),
-            LoadComposableNodes(
-                target_container=container_name_full,
-                condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('map'), '')),
-                composable_node_descriptions=[
-                    ComposableNode(
-                        package='nav2_map_server',
-                        plugin='nav2_map_server::MapServer',
-                        name='map_server',
-                        parameters=[configured_params,
-                                    {'yaml_filename': map_yaml_file}],
-                        remappings=remappings),
-                ],
-            ),
-            LoadComposableNodes(
-                target_container=container_name_full,
-                composable_node_descriptions=[
             ComposableNode(
                 package='nav2_amcl',
                 plugin='nav2_amcl::AmclNode',
@@ -200,12 +160,11 @@ def generate_launch_description():
                 package='nav2_lifecycle_manager',
                 plugin='nav2_lifecycle_manager::LifecycleManager',
                 name='lifecycle_manager_localization',
-                        parameters=[{'autostart': autostart,
+                parameters=[{'use_sim_time': use_sim_time,
+                             'autostart': autostart,
                              'node_names': lifecycle_nodes}]),
         ],
     )
-        ]
-    )
 
     # Create the launch description and populate
     ld = LaunchDescription()