diff --git a/nav2_bringup/bringup/launch/slam_launch.py b/nav2_bringup/bringup/launch/slam_launch.py index a32f68094be..8b5e4941a0d 100644 --- a/nav2_bringup/bringup/launch/slam_launch.py +++ b/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 diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py index 1f0638f81a0..c25fadb220c 100644 --- a/nav2_common/nav2_common/launch/__init__.py +++ b/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 diff --git a/nav2_common/nav2_common/launch/has_node_params.py b/nav2_common/nav2_common/launch/has_node_params.py new file mode 100644 index 00000000000..92f96790b5b --- /dev/null +++ b/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"