Added namespace support

ros-navigation · Jun 8, 2023 · 65c7621 · 65c7621
1 parent 7fb82f8
commit 65c7621
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 5 deletions.
diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py
@@ -22,9 +22,12 @@
 from launch.actions import DeclareLaunchArgument, GroupAction
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch.substitutions import NotEqualsSubstitution
 from launch_ros.actions import LoadComposableNodes, SetParameter
 from launch_ros.actions import Node
+from launch_ros.actions import PushRosNamespace
 from launch_ros.descriptions import ComposableNode
+from nav2_common.launch import RewrittenYaml
 
 
 def generate_launch_description():
@@ -34,6 +37,8 @@ def generate_launch_description():
     # Constant parameters
     lifecycle_nodes = ['collision_monitor']
     autostart = True
+    remappings = [('/tf', 'tf'),
+                  ('/tf_static', 'tf_static')]
 
     # Launch arguments
     # 1. Create the launch configuration variables
@@ -68,31 +73,46 @@ def generate_launch_description():
         'container_name', default_value='nav2_container',
         description='the name of conatiner that nodes will load in if use composition')
 
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key=namespace,
+        param_rewrites={},
+        convert_types=True)
+
+    # Declare launch commands
     load_nodes = GroupAction(
         condition=IfCondition(PythonExpression(['not ', use_composition])),
         actions=[
             SetParameter('use_sim_time', use_sim_time),
+            PushRosNamespace(
+                condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')),
+                namespace=namespace),
             Node(
                 package='nav2_lifecycle_manager',
                 executable='lifecycle_manager',
                 name='lifecycle_manager_collision_monitor',
                 output='screen',
                 emulate_tty=True,  # https://github.com/ros2/launch/issues/188
                 parameters=[{'autostart': autostart},
-                            {'node_names': lifecycle_nodes}]),
+                            {'node_names': lifecycle_nodes}],
+                remappings=remappings),
             Node(
                 package='nav2_collision_monitor',
                 executable='collision_monitor',
                 output='screen',
                 emulate_tty=True,  # https://github.com/ros2/launch/issues/188
-                parameters=[params_file])
+                parameters=[configured_params],
+                remappings=remappings)
         ]
     )
 
     load_composable_nodes = GroupAction(
         condition=IfCondition(use_composition),
         actions=[
             SetParameter('use_sim_time', use_sim_time),
+            PushRosNamespace(
+                condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')),
+                namespace=namespace),
             LoadComposableNodes(
                 target_container=container_name_full,
                 composable_node_descriptions=[
@@ -101,12 +121,14 @@ def generate_launch_description():
                         plugin='nav2_lifecycle_manager::LifecycleManager',
                         name='lifecycle_manager_collision_monitor',
                         parameters=[{'autostart': autostart},
-                                    {'node_names': lifecycle_nodes}]),
+                                    {'node_names': lifecycle_nodes}],
+                        remappings=remappings),
                     ComposableNode(
                         package='nav2_collision_monitor',
                         plugin='nav2_collision_monitor::CollisionMonitor',
                         name='collision_monitor',
-                        parameters=[params_file]),
+                        parameters=[configured_params],
+                        remappings=remappings)
                 ],
             )
         ]

diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml
@@ -50,7 +50,7 @@ collision_monitor:
     observation_sources: ["scan"]
     scan:
       type: "scan"
-      topic: "/scan"
+      topic: "scan"
     pointcloud:
       type: "pointcloud"
       topic: "/intel_realsense_r200_depth/points"