[multirobot] Add namespacing to nav stack. (#1147)

* Extending `rewritten_yaml` to also replace keys. * Adding a node launcher that puts a condition to applying the remappings. * Add namespacing of launch files. * Update namespace of costmap and passing of correct namespace by planner & controller.
ros-navigation · Sep 25, 2019 · 226f06c · 226f06c
1 parent a0ee02e
commit 226f06c
Show file tree

Hide file tree

Showing 13 changed files with 604 additions and 51 deletions.
diff --git a/nav2_bringup/bringup/launch/nav2_bringup_launch.py b/nav2_bringup/bringup/launch/nav2_bringup_launch.py
@@ -16,11 +16,12 @@
 
 from ament_index_python.packages import get_package_prefix, get_package_share_directory
 
+from nav2_common.launch import Node
+
 from launch import LaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+from launch.substitutions import LaunchConfiguration
 from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, IncludeLaunchDescription
-from launch_ros.actions import Node
 
 
 def generate_launch_description():
@@ -29,15 +30,32 @@ def generate_launch_description():
     launch_dir = os.path.join(bringup_dir, 'launch')
 
     # Create the launch configuration variables
+    namespace = LaunchConfiguration('namespace')
     map_yaml_file = LaunchConfiguration('map')
     use_sim_time = LaunchConfiguration('use_sim_time')
     params_file = LaunchConfiguration('params_file')
     bt_xml_file = LaunchConfiguration('bt_xml_file')
     autostart = LaunchConfiguration('autostart')
+    use_remappings = LaunchConfiguration('use_remappings')
+
+    # TODO(orduno) Remove once `PushNodeRemapping` is resolved
+    #              https://github.com/ros2/launch_ros/issues/56
+    remappings = [((namespace, '/tf'), '/tf'),
+                  ((namespace, '/tf_static'), '/tf_static'),
+                  ('/scan', 'scan'),
+                  ('/tf', 'tf'),
+                  ('/tf_static', 'tf_static'),
+                  ('/cmd_vel', 'cmd_vel'),
+                  ('/map', 'map')]
 
     stdout_linebuf_envvar = SetEnvironmentVariable(
         'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
 
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Top-level namespace')
+
     # Declare the launch arguments
     declare_map_yaml_cmd = DeclareLaunchArgument(
         'map',
@@ -64,22 +82,30 @@ def generate_launch_description():
         'autostart', default_value='true',
         description='Automatically startup the nav2 stack')
 
+    declare_use_remappings_cmd = DeclareLaunchArgument(
+        'use_remappings', default_value='false',
+        description='Arguments to pass to all nodes launched by the file')
+
     # Specify the actions
     start_localization_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(os.path.join(launch_dir, 'nav2_localization_launch.py')),
-        launch_arguments={'map': map_yaml_file,
+        launch_arguments={'namespace': namespace,
+                          'map': map_yaml_file,
                           'use_sim_time': use_sim_time,
                           'autostart': autostart,
                           'params_file': params_file,
-                          'use_lifecycle_mgr': 'false'}.items())
+                          'use_lifecycle_mgr': 'false',
+                          'use_remappings': use_remappings}.items())
 
     start_navigation_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(os.path.join(launch_dir, 'nav2_navigation_launch.py')),
-        launch_arguments={'use_sim_time': use_sim_time,
+        launch_arguments={'namespace': namespace,
+                          'use_sim_time': use_sim_time,
                           'autostart': autostart,
                           'params_file': params_file,
                           'bt_xml_file': bt_xml_file,
                           'use_lifecycle_mgr': 'false',
+                          'use_remappings': use_remappings,
                           'map_subscribe_transient_local': 'true'}.items())
 
     start_lifecycle_manager_cmd = Node(
@@ -102,11 +128,13 @@ def generate_launch_description():
     ld.add_action(stdout_linebuf_envvar)
 
     # Declare the launch options
+    ld.add_action(declare_namespace_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_bt_xml_cmd)
+    ld.add_action(declare_use_remappings_cmd)
 
     # Add the actions to launch all of the navigation nodes
     ld.add_action(start_lifecycle_manager_cmd)

diff --git a/nav2_bringup/bringup/launch/nav2_localization_launch.py b/nav2_bringup/bringup/launch/nav2_localization_launch.py
@@ -17,38 +17,58 @@
 from ament_index_python.packages import get_package_share_directory
 
 from nav2_common.launch import RewrittenYaml
+from nav2_common.launch import Node
 
 from launch import LaunchDescription
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration
 from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
-from launch_ros.actions import Node
 
 
 def generate_launch_description():
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
 
+    namespace = LaunchConfiguration('namespace')
     map_yaml_file = LaunchConfiguration('map')
     use_sim_time = LaunchConfiguration('use_sim_time')
     autostart = LaunchConfiguration('autostart')
     params_file = LaunchConfiguration('params_file')
     use_lifecycle_mgr = LaunchConfiguration('use_lifecycle_mgr')
+    use_remappings = LaunchConfiguration('use_remappings')
+
+    # TODO(orduno) Remove once `PushNodeRemapping` is resolved
+    #              https://github.com/ros2/launch_ros/issues/56
+    remappings = [((namespace, '/tf'), '/tf'),
+                  ((namespace, '/tf_static'), '/tf_static'),
+                  ('/scan', 'scan'),
+                  ('/tf', 'tf'),
+                  ('/tf_static', 'tf_static'),
+                  ('/cmd_vel', 'cmd_vel'),
+                  ('/map', 'map'),
+                  ('/goal_pose', 'goal_pose')]
 
     # Create our own temporary YAML files that include substitutions
     param_substitutions = {
         'use_sim_time': use_sim_time,
         'yaml_filename': map_yaml_file}
 
+    namespace_substitutions = {'navigation_namespace': namespace}
+
     configured_params = RewrittenYaml(
         source_file=params_file,
-        rewrites=param_substitutions,
+        param_rewrites=param_substitutions,
+        key_rewrites=namespace_substitutions,
         convert_types=True)
 
     return LaunchDescription([
         # Set env var to print messages to stdout immediately
         SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
 
+        DeclareLaunchArgument(
+            'namespace', default_value='',
+            description='Top-level namespace'),
+
         DeclareLaunchArgument(
             'map',
             default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'),
@@ -71,19 +91,27 @@ def generate_launch_description():
             'use_lifecycle_mgr', default_value='true',
             description='Whether to launch the lifecycle manager'),
 
+        DeclareLaunchArgument(
+            'use_remappings', default_value='false',
+            description='Arguments to pass to all nodes launched by the file'),
+
         Node(
             package='nav2_map_server',
             node_executable='map_server',
             node_name='map_server',
             output='screen',
-            parameters=[configured_params]),
+            parameters=[configured_params],
+            use_remappings=IfCondition(use_remappings),
+            remappings=remappings),
 
         Node(
             package='nav2_amcl',
             node_executable='amcl',
             node_name='amcl',
             output='screen',
-            parameters=[configured_params]),
+            parameters=[configured_params],
+            use_remappings=IfCondition(use_remappings),
+            remappings=remappings),
 
         Node(
             condition=IfCondition(use_lifecycle_mgr),

diff --git a/nav2_bringup/bringup/launch/nav2_navigation_launch.py b/nav2_bringup/bringup/launch/nav2_navigation_launch.py
@@ -17,41 +17,61 @@
 from ament_index_python.packages import get_package_prefix, get_package_share_directory
 
 from nav2_common.launch import RewrittenYaml
+from nav2_common.launch import Node
 
 from launch import LaunchDescription
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
 from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
-from launch_ros.actions import Node
 
 
 def generate_launch_description():
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
 
+    namespace = LaunchConfiguration('namespace')
     use_sim_time = LaunchConfiguration('use_sim_time')
     autostart = LaunchConfiguration('autostart')
     params_file = LaunchConfiguration('params_file')
     bt_xml_file = LaunchConfiguration('bt_xml_file')
     use_lifecycle_mgr = LaunchConfiguration('use_lifecycle_mgr')
+    use_remappings = LaunchConfiguration('use_remappings')
     map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')
 
+    # TODO(orduno) Remove once `PushNodeRemapping` is resolved
+    #              https://github.com/ros2/launch_ros/issues/56
+    remappings = [((namespace, '/tf'), '/tf'),
+                  ((namespace, '/tf_static'), '/tf_static'),
+                  ('/scan', 'scan'),
+                  ('/tf', 'tf'),
+                  ('/tf_static', 'tf_static'),
+                  ('/cmd_vel', 'cmd_vel'),
+                  ('/map', 'map'),
+                  ('/goal_pose', 'goal_pose')]
+
     # Create our own temporary YAML files that include substitutions
     param_substitutions = {
         'use_sim_time': use_sim_time,
         'bt_xml_filename': bt_xml_file,
         'autostart': autostart,
         'map_subscribe_transient_local': map_subscribe_transient_local}
 
+    namespace_substitutions = {'navigation_namespace': namespace}
+
     configured_params = RewrittenYaml(
             source_file=params_file,
-            rewrites=param_substitutions,
+            param_rewrites=param_substitutions,
+            key_rewrites=namespace_substitutions,
             convert_types=True)
 
     return LaunchDescription([
         # Set env var to print messages to stdout immediately
         SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
 
+        DeclareLaunchArgument(
+            'namespace', default_value='',
+            description='Top-level namespace'),
+
         DeclareLaunchArgument(
             'use_sim_time', default_value='false',
             description='Use simulation (Gazebo) clock if true'),
@@ -75,6 +95,10 @@ def generate_launch_description():
             'use_lifecycle_mgr', default_value='true',
             description='Whether to launch the lifecycle manager'),
 
+        DeclareLaunchArgument(
+            'use_remappings', default_value='false',
+            description='Arguments to pass to all nodes launched by the file'),
+
         DeclareLaunchArgument(
             'map_subscribe_transient_local', default_value='false',
             description='Whether to set the map subscriber QoS to transient local'),
@@ -83,28 +107,36 @@ def generate_launch_description():
             package='nav2_controller',
             node_executable='controller_server',
             output='screen',
-            parameters=[configured_params]),
+            parameters=[configured_params],
+            use_remappings=IfCondition(use_remappings),
+            remappings=remappings),
 
         Node(
             package='nav2_planner',
             node_executable='planner_server',
             node_name='planner_server',
             output='screen',
-            parameters=[configured_params]),
+            parameters=[configured_params],
+            use_remappings=IfCondition(use_remappings),
+            remappings=remappings),
 
         Node(
             package='nav2_recoveries',
             node_executable='recoveries_node',
             node_name='recoveries',
             output='screen',
-            parameters=[{'use_sim_time': use_sim_time}]),
+            parameters=[{'use_sim_time': use_sim_time}],
+            use_remappings=IfCondition(use_remappings),
+            remappings=remappings),
 
         Node(
             package='nav2_bt_navigator',
             node_executable='bt_navigator',
             node_name='bt_navigator',
             output='screen',
-            parameters=[configured_params]),
+            parameters=[configured_params],
+            use_remappings=IfCondition(use_remappings),
+            remappings=remappings),
 
         Node(
             condition=IfCondition(use_lifecycle_mgr),