If provided param file has no slam_toolbox params, don't forward it

nav2_bringup/bringup/launch/slam_launch.py

@@ -17,11 +17,14 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription,
+                            PopLaunchConfigurations, PushLaunchConfigurations,
+                            UnsetLaunchConfiguration)
+from launch.conditions import UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-from nav2_common.launch import RewrittenYaml
+from nav2_common.launch import HasNodeParams, RewrittenYaml
 
 
 def generate_launch_description():
@@ -70,9 +73,6 @@ def generate_launch_description():
         description='Automatically startup the nav2 stack')
 
     # Nodes launching commands
-    start_slam_toolbox_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(slam_launch_file),
-        launch_arguments={'use_sim_time': use_sim_time}.items())
 
     start_map_saver_server_cmd = Node(
             package='nav2_map_server',
@@ -89,6 +89,30 @@ def generate_launch_description():
                         {'autostart': autostart},
                         {'node_names': lifecycle_nodes}])
 
+    # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file'
+    # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load
+    # the default file
+    has_slam_toolbox_params = HasNodeParams(source_file=params_file,
+                                            node_name='slam_toolbox')
+    # Push (or save) current LaunchConfiguration
+    push_launch_config = PushLaunchConfigurations(
+            condition=UnlessCondition(has_slam_toolbox_params))
+
+    # Remove params_file LaunchConfiguration before passing to slam_toolbox
+    remove_params_file = UnsetLaunchConfiguration(
+            'params_file', condition=UnlessCondition(has_slam_toolbox_params))
+
+    # Include slam_toolbox LaunchDescription, the params_file will be pased automatically
+    # unless it was Unset before.
+    # See https://github.com/ros2/launch/issues/313
+    start_slam_toolbox_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(slam_launch_file),
+        launch_arguments={'use_sim_time': use_sim_time}.items())
+
+    # Pop (or load) previous LaunchConfiguration, resetting the state of params_file
+    pop_launch_config = PopLaunchConfigurations(
+            condition=UnlessCondition(has_slam_toolbox_params))
+
     ld = LaunchDescription()
 
     # Declare the launch options
@@ -97,11 +121,14 @@ def generate_launch_description():
     ld.add_action(declare_use_sim_time_cmd)
     ld.add_action(declare_autostart_cmd)
 
-    # Running SLAM Toolbox
-    ld.add_action(start_slam_toolbox_cmd)
-
     # Running Map Saver Server
     ld.add_action(start_map_saver_server_cmd)
     ld.add_action(start_lifecycle_manager_cmd)
 
+    # Running SLAM Toolbox
+    ld.add_action(push_launch_config)
+    ld.add_action(remove_params_file)
+    ld.add_action(start_slam_toolbox_cmd)
+    ld.add_action(pop_launch_config)
+
     return ld

nav2_common/nav2_common/launch/__init__.py

@@ -12,5 +12,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from .has_node_params import HasNodeParams
 from .rewritten_yaml import RewrittenYaml
 from .replace_string import ReplaceString

nav2_common/nav2_common/launch/has_node_params.py

@@ -0,0 +1,60 @@
+# Copyright (c) 2021 PAL Robotics S.L.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import List
+from typing import Text
+
+import yaml
+import launch
+
+import sys # delete this
+
+class HasNodeParams(launch.Substitution):
+  """
+  Substitution that checks if a param file contains parameters for a node
+
+  Used in launch system
+  """
+
+  def __init__(self,
+    source_file: launch.SomeSubstitutionsType,
+    node_name: Text) -> None:
+    super().__init__()
+    """
+    Construct the substitution
+
+    :param: source_file the parameter YAML file
+    :param: node_name the name of the node to check
+    """
+
+    from launch.utilities import normalize_to_list_of_substitutions  # import here to avoid loop
+    self.__source_file = normalize_to_list_of_substitutions(source_file)
+    self.__node_name = node_name
+
+  @property
+  def name(self) -> List[launch.Substitution]:
+    """Getter for name."""
+    return self.__source_file
+
+  def describe(self) -> Text:
+    """Return a description of this substitution as a string."""
+    return ''
+
+  def perform(self, context: launch.LaunchContext) -> Text:
+    yaml_filename = launch.utilities.perform_substitutions(context, self.name)
+    data = yaml.safe_load(open(yaml_filename, 'r'))
+
+    if self.__node_name in data.keys():
+        return "True"
+    return "False"