[multirobot - Part3] Add multi-robot launching

nav2_bringup/bringup/launch/nav2_localization_launch.py

@@ -53,12 +53,10 @@ def generate_launch_description():
         'use_sim_time': use_sim_time,
         'yaml_filename': map_yaml_file}
 
-    namespace_substitutions = {'navigation_namespace': namespace}
-
     configured_params = RewrittenYaml(
         source_file=params_file,
+        root_key=namespace,
         param_rewrites=param_substitutions,
-        key_rewrites=namespace_substitutions,
         convert_types=True)
 
     return LaunchDescription([

nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py

@@ -0,0 +1,171 @@
+# 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.
+
+''' This is an example on how to create a launch file for spawning multiple robots into Gazebo
+    and launch multiple instances of the navigation stack, each controlling one robot.
+    The robots co-exist on a shared environment and are controlled by independent nav stacks '''
+
+import os
+
+from ament_index_python.packages import get_package_prefix
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription,
+    LogInfo, GroupAction, ExecuteProcess)
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch_ros.actions import PushRosNamespace
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('nav2_bringup')
+
+    # Names and poses of the robots
+    robots = [
+        {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01},
+        {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01}]
+
+    # Simulation settings
+    world = LaunchConfiguration('world')
+    simulator = LaunchConfiguration('simulator')
+
+    # On this example all robots are launched with the same settings
+    map_yaml_file = LaunchConfiguration('map')
+    params_file = LaunchConfiguration('params_file')
+    bt_xml_file = LaunchConfiguration('bt_xml_file')
+    log_settings = LaunchConfiguration('log_settings', default='true')
+
+    # Declare the launch arguments
+    declare_world_cmd = DeclareLaunchArgument(
+        'world',
+        default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'),
+        description='Full path to world file to load')
+
+    declare_simulator_cmd = DeclareLaunchArgument(
+        'simulator',
+        default_value='gazebo',
+        description='The simulator to use (gazebo or gzserver)')
+
+    declare_map_yaml_cmd = DeclareLaunchArgument(
+        'map',
+        default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'),
+        description='Full path to map file to load')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+    declare_bt_xml_cmd = DeclareLaunchArgument(
+        'bt_xml_file',
+        default_value=os.path.join(
+            get_package_prefix('nav2_bt_navigator'),
+            'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
+        description='Full path to the behavior tree xml file to use')
+
+   # Start Gazebo with plugin providing the robot spawing service
+    start_gazebo_cmd = ExecuteProcess(
+        cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', world],
+        output='screen')
+
+    # Define commands for spawing the robots into Gazebo
+    spawn_robots_cmds = []
+    for robot in robots:
+        spawn_robots_cmds.append(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'spawn_robot_launch.py')),
+                launch_arguments={
+                                  'x_pose': TextSubstitution(text=str(robot['x_pose'])),
+                                  'y_pose': TextSubstitution(text=str(robot['y_pose'])),
+                                  'z_pose': TextSubstitution(text=str(robot['z_pose'])),
+                                  'robot_name': robot['name'],
+                                  'turtlebot_type': TextSubstitution(text='waffle')
+                                  }.items()))
+
+    # Define commands for launching the navigation instances
+    nav_instances_cmds = []
+    for robot in robots:
+        group = GroupAction([
+            # TODO(orduno) Each `action.Node` within the `localization` and `navigation` launch
+            #              files has two versions, one with the required remaps and another without.
+            #              The `use_remappings` flag specifies which runs.
+            #              A better mechanism would be to have a PushNodeRemapping() action:
+            #              https://github.com/ros2/launch_ros/issues/56
+            #              For more on why we're remapping topics, see the note below
+
+            # PushNodeRemapping(remappings)
+
+            # Instances use the robot's name for namespace
+            PushRosNamespace(robot['name']),
+
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'nav2_tb3_simulation_launch.py')),
+                launch_arguments={
+                                  #TODO(orduno) might not be necessary to pass the robot name
+                                  'namespace': robot['name'],
+                                  'map_yaml_file': map_yaml_file,
+                                  'use_sim_time': 'True',
+                                  'params_file': params_file,
+                                  'bt_xml_file': bt_xml_file,
+                                  'autostart': 'False',
+                                  'use_remappings': 'True',
+                                  'use_simulator': 'False'}.items()),
+
+            LogInfo(
+                condition=IfCondition(log_settings),
+                msg=['Launching ', robot['name']]),
+            LogInfo(
+                condition=IfCondition(log_settings),
+                msg=[robot['name'], ' map yaml: ', map_yaml_file]),
+            LogInfo(
+                condition=IfCondition(log_settings),
+                msg=[robot['name'], ' params yaml: ', params_file]),
+            LogInfo(
+                condition=IfCondition(log_settings),
+                msg=[robot['name'], ' behavior tree xml: ', bt_xml_file])
+        ])
+
+        nav_instances_cmds.append(group)
+
+    # A note on the `remappings` variable defined above and the fact it's passed as a node arg.
+    # A few topics have fully qualified names (have a leading '/'), these need to be remapped
+    # to relative ones so the node's namespace can be prepended.
+    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
+    # for multi-robot transforms:
+    # https://github.com/ros/geometry2/issues/32
+    # https://github.com/ros/robot_state_publisher/pull/30
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_simulator_cmd)
+    ld.add_action(declare_world_cmd)
+    ld.add_action(declare_map_yaml_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_bt_xml_cmd)
+
+    # Add the actions to start gazebo, robots and simulations
+    ld.add_action(start_gazebo_cmd)
+
+    for spawn_robot_cmd in spawn_robots_cmds:
+        ld.add_action(spawn_robot_cmd)
+
+    for simulation_instance_cmd in nav_instances_cmds:
+        ld.add_action(simulation_instance_cmd)
+
+    return ld

nav2_bringup/bringup/launch/nav2_navigation_launch.py

@@ -56,12 +56,10 @@ def generate_launch_description():
         'autostart': autostart,
         'map_subscribe_transient_local': map_subscribe_transient_local}
 
-    namespace_substitutions = {'navigation_namespace': namespace}
-
     configured_params = RewrittenYaml(
             source_file=params_file,
+            root_key=namespace,
             param_rewrites=param_substitutions,
-            key_rewrites=namespace_substitutions,
             convert_types=True)
 
     return LaunchDescription([

nav2_bringup/bringup/launch/spawn_robot_launch.py

@@ -28,9 +28,10 @@ def generate_launch_description():
             node_executable='spawn_turtlebot',
             output='screen',
             arguments=[
-                    launch.substitutions.LaunchConfiguration('robot_name'),
-                    launch.substitutions.LaunchConfiguration('robot_name'),
-                    launch.substitutions.LaunchConfiguration('x_pose'),
-                    launch.substitutions.LaunchConfiguration('y_pose'),
-                    launch.substitutions.LaunchConfiguration('z_pose')]),
+                '--robot_name', launch.substitutions.LaunchConfiguration('robot_name'),
+                '--robot_namespace', launch.substitutions.LaunchConfiguration('robot_name'),
+                '--turtlebot_type', launch.substitutions.LaunchConfiguration('turtlebot_type'),
+                '-x', launch.substitutions.LaunchConfiguration('x_pose'),
+                '-y', launch.substitutions.LaunchConfiguration('y_pose'),
+                '-z', launch.substitutions.LaunchConfiguration('z_pose')]),
     ])

nav2_bringup/bringup/worlds/world_only.model

@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <scene>
+      <shadows>false</shadows>
+    </scene>
+
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+
+    <physics type="ode">
+      <real_time_update_rate>1000.0</real_time_update_rate>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>150</iters>
+          <precon_iters>0</precon_iters>
+          <sor>1.400000</sor>
+          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+        </solver>
+        <constraints>
+          <cfm>0.00001</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
+          <contact_surface_layer>0.01000</contact_surface_layer>
+        </constraints>
+      </ode>
+    </physics>
+
+    <model name="turtlebot3_world">
+      <static>1</static>
+      <include>
+        <uri>model://turtlebot3_world</uri>
+      </include>
+    </model>
+
+  </world>
+</sdf>

nav2_bringup/spawn_turtlebot3/src/spawn_turtlebot3/spawn_turtlebot.py

@@ -28,7 +28,8 @@ def main():
                         help='the y component of the initial position [meters]')
     parser.add_argument('-z', type=float, default=0,
                         help='the z component of the initial position [meters]')
-    args = parser.parse_args()
+
+    args, unknown = parser.parse_known_args()
 
     # Start node
     rclpy.init()

nav2_common/nav2_common/launch/rewritten_yaml.py

@@ -16,6 +16,7 @@
 from typing import List
 from typing import Text
 from typing import Optional
+
 import yaml
 import tempfile
 import launch
@@ -33,15 +34,25 @@ def setValue(self, value):
 
 class RewrittenYaml(launch.Substitution):
   """
-  Substitution that modifies the given Yaml file.
+  Substitution that modifies the given YAML file.
   """
 
   def __init__(self,
     source_file: launch.SomeSubstitutionsType,
     param_rewrites: Dict,
+    root_key: Optional[launch.SomeSubstitutionsType] = None,
     key_rewrites: Optional[Dict] = None,
     convert_types = False) -> None:
     super().__init__()
+    """
+    Construct the substitution
+
+    :param: source_file the original YAML file to modify
+    :param: param_rewrites mappings to replace
+    :param: root_key if provided, the contents are placed under this key
+    :param: key_rewrites keys of mappings to replace
+    :param: convert_types whether to attempt converting the string to a number or boolean
+    """
 
     from launch.utilities import normalize_to_list_of_substitutions  # import here to avoid loop
     self.__source_file = normalize_to_list_of_substitutions(source_file)
@@ -53,6 +64,8 @@ def __init__(self,
     if key_rewrites is not None:
         for key in key_rewrites:
           self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key])
+    if root_key is not None:
+        self.__root_key = normalize_to_list_of_substitutions(root_key)
 
   @property
   def name(self) -> List[launch.Substitution]:
@@ -70,6 +83,10 @@ def perform(self, context: launch.LaunchContext) -> Text:
     data = yaml.safe_load(open(yaml_filename, 'r'))
     self.substitute_params(data, param_rewrites)
     self.substitute_keys(data, keys_rewrites)
+    if self.__root_key is not None:
+        root_key = launch.utilities.perform_substitutions(context, self.__root_key)
+        if root_key:
+            data = {root_key: data}
     yaml.dump(data, rewritten_yaml)
     rewritten_yaml.close()
     return rewritten_yaml.name