Make it possible to launch multi-robot which rewrites <robot_namespac…

…e> to namespace.

Automatically apply namespace on specific target topic path in costmap plugins.
re-utilize navigation_launch file.

add new params file for multi-robot -> nav2_multirobot_params.yaml

nav2_bringup/launch/bringup_multirobot_launch.py

@@ -0,0 +1,141 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.actions import OpaqueFunction, GroupAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from nav2_common.launch import ReplaceString
+
+def generate_bringup_multirobot(context):
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+    bringup_launch_script = PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py'))
+
+    slam = LaunchConfiguration('slam')
+    map_yaml_file = LaunchConfiguration('map')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    params_file = LaunchConfiguration('params_file')
+    autostart = LaunchConfiguration('autostart')
+    use_composition = LaunchConfiguration('use_composition')
+    use_respawn = LaunchConfiguration('use_respawn')
+    log_level = LaunchConfiguration('log_level')
+
+    num_of_robots = context.launch_configurations['number_of_robots']
+    namespace = context.launch_configurations['namespace']
+    bringup_cmd_group = []
+    print(f"number_of_robots = {num_of_robots}")
+
+    for n in range(int(num_of_robots)):
+        new_namespace = [namespace, TextSubstitution(text=str(n))]
+        # Specify the actions
+        bringup_with_ns_cmd_params = IncludeLaunchDescription(
+            bringup_launch_script,
+            launch_arguments=({'namespace': new_namespace,
+                               'slam': slam,
+                               'map': map_yaml_file,
+                               'use_sim_time': use_sim_time,
+                               'autostart': autostart,
+                               'use_respawn': use_respawn,
+                               'use_composition': use_composition,
+                               'log_level': log_level,
+                               'params_file': params_file}).items())
+        bringup_cmd_group.append(bringup_with_ns_cmd_params)
+
+    return bringup_cmd_group
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('nav2_bringup')
+
+    # Create the launch configuration variables
+    number_of_robots = LaunchConfiguration('number_of_robots')
+    namespace = LaunchConfiguration('namespace')
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
+
+    declare_num_of_robots_cmd = DeclareLaunchArgument(
+        'number_of_robots',
+        default_value='3',
+        description='The number of robots to bringup robots')
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='robot',
+        description='Top-level prefix for namespace, ex) robot -> robot1, robot2')
+
+    declare_slam_cmd = DeclareLaunchArgument(
+        'slam',
+        default_value='False',
+        description='Whether run a SLAM')
+
+    declare_map_yaml_cmd = DeclareLaunchArgument(
+        'map',
+        default_value='',
+        description='Full path to map yaml file to load')
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation (Gazebo) clock if true')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart', default_value='true',
+        description='Automatically startup the nav2 stack')
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition', default_value='True',
+        description='Whether to use composed bringup')
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        'log_level', default_value='info',
+        description='log level')
+
+    bringup_multirobot_group_cmd = OpaqueFunction(function=generate_bringup_multirobot)
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_num_of_robots_cmd)
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_slam_cmd)
+    ld.add_action(declare_map_yaml_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+    ld.add_action(bringup_multirobot_group_cmd)
+
+    return ld

nav2_bringup/launch/bringup_with_ns_launch.py

@@ -0,0 +1,126 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from nav2_common.launch import ReplaceString
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+
+    # Create the launch configuration variables
+    namespace = LaunchConfiguration('namespace')
+    slam = LaunchConfiguration('slam')
+    map_yaml_file = LaunchConfiguration('map')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    params_file = LaunchConfiguration('params_file')
+    autostart = LaunchConfiguration('autostart')
+    use_composition = LaunchConfiguration('use_composition')
+    use_respawn = LaunchConfiguration('use_respawn')
+    log_level = LaunchConfiguration('log_level')
+
+    # '<robot_namespace>' keyword shall be replaced by 'namespace' launch argument
+    # in config file 'nav2_namespaced_params.yaml' as a default.
+    # User defined config file should containe '<robot_namespace>' keyword
+    # for the proper parameter wherever you want to get parameters thet namespace applied.
+    namespaced_replaced_params_file = ReplaceString(
+        source_file=params_file,
+        replacements={'<robot_namespace>': (namespace)})
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='robot0',
+        description='Top-level namespace')
+
+    declare_slam_cmd = DeclareLaunchArgument(
+        'slam',
+        default_value='False',
+        description='Whether run a SLAM')
+
+    declare_map_yaml_cmd = DeclareLaunchArgument(
+        'map',
+        default_value='',
+        description='Full path to map yaml file to load')
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation (Gazebo) clock if true')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart', default_value='true',
+        description='Automatically startup the nav2 stack')
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition', default_value='True',
+        description='Whether to use composed bringup')
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        'log_level', default_value='info',
+        description='log level')
+
+    # Specify the actions
+    bringup_with_namespace_cmd_params = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')),
+            launch_arguments={'namespace': namespace,
+                              'use_namespace': True,
+                              'slam': slam,
+                              'map': map_yaml_file,
+                              'use_sim_time': use_sim_time,
+                              'autostart': autostart,
+                              'use_respawn': use_respawn,
+                              'use_composition': use_composition,
+                              'log_level': log_level,
+                              'params_file': namespaced_replaced_params_file}.items())
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_slam_cmd)
+    ld.add_action(declare_map_yaml_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(bringup_with_namespace_cmd_params)
+
+    return ld