WIP single robot namespacing working

Signed-off-by: Luca Della Vedova <[email protected]>

nav2_bringup/launch/rviz_launch.py

@@ -18,12 +18,10 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
-from launch.conditions import IfCondition, UnlessCondition
 from launch.event_handlers import OnProcessExit
 from launch.events import Shutdown
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-from nav2_common.launch import ReplaceString
 
 
 def generate_launch_description():
@@ -32,7 +30,6 @@ def generate_launch_description():
 
     # Create the launch configuration variables
     namespace = LaunchConfiguration('namespace')
-    use_namespace = LaunchConfiguration('use_namespace')
     rviz_config_file = LaunchConfiguration('rviz_config')
     use_sim_time = LaunchConfiguration('use_sim_time')
 
@@ -46,12 +43,6 @@ def generate_launch_description():
         ),
     )
 
-    declare_use_namespace_cmd = DeclareLaunchArgument(
-        'use_namespace',
-        default_value='false',
-        description='Whether to apply a namespace to the navigation stack',
-    )
-
     declare_rviz_config_file_cmd = DeclareLaunchArgument(
         'rviz_config',
         default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'),
@@ -65,68 +56,37 @@ def generate_launch_description():
 
     # Launch rviz
     start_rviz_cmd = Node(
-        condition=UnlessCondition(use_namespace),
         package='rviz2',
         executable='rviz2',
+        namespace=namespace,
         arguments=['-d', rviz_config_file],
         output='screen',
         parameters=[{'use_sim_time': use_sim_time}],
-    )
-
-    namespaced_rviz_config_file = ReplaceString(
-        source_file=rviz_config_file,
-        replacements={'<robot_namespace>': ('/', namespace)},
-    )
-
-    start_namespaced_rviz_cmd = Node(
-        condition=IfCondition(use_namespace),
-        package='rviz2',
-        executable='rviz2',
-        namespace=namespace,
-        arguments=['-d', namespaced_rviz_config_file],
-        parameters=[{'use_sim_time': use_sim_time}],
-        output='screen',
         remappings=[
-            ('/map', 'map'),
             ('/tf', 'tf'),
             ('/tf_static', 'tf_static'),
-            ('/goal_pose', 'goal_pose'),
-            ('/clicked_point', 'clicked_point'),
-            ('/initialpose', 'initialpose'),
         ],
     )
 
     exit_event_handler = RegisterEventHandler(
-        condition=UnlessCondition(use_namespace),
         event_handler=OnProcessExit(
             target_action=start_rviz_cmd,
             on_exit=EmitEvent(event=Shutdown(reason='rviz exited')),
         ),
     )
 
-    exit_event_handler_namespaced = RegisterEventHandler(
-        condition=IfCondition(use_namespace),
-        event_handler=OnProcessExit(
-            target_action=start_namespaced_rviz_cmd,
-            on_exit=EmitEvent(event=Shutdown(reason='rviz exited')),
-        ),
-    )
-
     # Create the launch description and populate
     ld = LaunchDescription()
 
     # Declare the launch options
     ld.add_action(declare_namespace_cmd)
-    ld.add_action(declare_use_namespace_cmd)
     ld.add_action(declare_rviz_config_file_cmd)
     ld.add_action(declare_use_sim_time_cmd)
 
     # Add any conditioned actions
     ld.add_action(start_rviz_cmd)
-    ld.add_action(start_namespaced_rviz_cmd)
 
     # Add other nodes and processes we need
     ld.add_action(exit_event_handler)
-    ld.add_action(exit_event_handler_namespaced)
 
     return ld

nav2_bringup/params/nav2_params.yaml

@@ -42,7 +42,7 @@ bt_navigator:
   ros__parameters:
     global_frame: map
     robot_base_frame: base_link
-    odom_topic: /odom
+    odom_topic: odom
     bt_loop_duration: 10
     default_server_timeout: 20
     wait_for_service_timeout: 1000
@@ -211,7 +211,7 @@ local_costmap:
         mark_threshold: 0
         observation_sources: scan
         scan:
-          topic: /scan
+          topic: scan
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -241,7 +241,7 @@ global_costmap:
         enabled: True
         observation_sources: scan
         scan:
-          topic: /scan
+          topic: scan
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -362,7 +362,7 @@ collision_monitor:
     FootprintApproach:
       type: "polygon"
       action_type: "approach"
-      footprint_topic: "/local_costmap/published_footprint"
+      footprint_topic: "local_costmap/published_footprint"
       time_before_collision: 1.2
       simulation_time_step: 0.1
       min_points: 6

nav2_bringup/rviz/nav2_default_view.rviz

@@ -58,7 +58,7 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /robot_description
+        Value: robot_description
       Enabled: false
       Links:
         All Links Enabled: true
@@ -148,7 +148,7 @@ Visualization Manager:
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Best Effort
-        Value: /scan
+        Value: scan
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
@@ -181,7 +181,7 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Best Effort
-        Value: /mobile_base/sensors/bumper_pointcloud
+        Value: mobile_base/sensors/bumper_pointcloud
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
@@ -199,13 +199,13 @@ Visualization Manager:
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /map
+        Value: map
       Update Topic:
         Depth: 5
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /map_updates
+        Value: map_updates
       Use Timestamp: false
       Value: true
     - Alpha: 1
@@ -222,7 +222,7 @@ Visualization Manager:
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Best Effort
-        Value: /particle_cloud
+        Value: particle_cloud
       Value: true
     - Class: rviz_common/Group
       Displays:
@@ -240,13 +240,13 @@ Visualization Manager:
             Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /global_costmap/costmap
+            Value: global_costmap/costmap
           Update Topic:
             Depth: 5
             Durability Policy: Volatile
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /global_costmap/costmap_updates
+            Value: global_costmap/costmap_updates
           Use Timestamp: false
           Value: true
         - Alpha: 0.30000001192092896
@@ -263,13 +263,13 @@ Visualization Manager:
             Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /downsampled_costmap
+            Value: downsampled_costmap
           Update Topic:
             Depth: 5
             Durability Policy: Volatile
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /downsampled_costmap_updates
+            Value: downsampled_costmap_updates
           Use Timestamp: false
           Value: true
         - Alpha: 1
@@ -298,7 +298,7 @@ Visualization Manager:
             Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /plan
+            Value: plan
           Value: true
         - Alpha: 1
           Autocompute Intensity Bounds: true
@@ -329,7 +329,7 @@ Visualization Manager:
             Durability Policy: Volatile
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /global_costmap/voxel_marked_cloud
+            Value: global_costmap/voxel_marked_cloud
           Use Fixed Frame: true
           Use rainbow: true
           Value: true
@@ -344,7 +344,7 @@ Visualization Manager:
             Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /global_costmap/published_footprint
+            Value: global_costmap/published_footprint
           Value: false
       Enabled: true
       Name: Global Planner
@@ -364,13 +364,13 @@ Visualization Manager:
             Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /local_costmap/costmap
+            Value: local_costmap/costmap
           Update Topic:
             Depth: 5
             Durability Policy: Volatile
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /local_costmap/costmap_updates
+            Value: local_costmap/costmap_updates
           Use Timestamp: false
           Value: true
         - Alpha: 1
@@ -399,7 +399,7 @@ Visualization Manager:
             Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /local_plan
+            Value: local_plan
           Value: true
         - Class: rviz_default_plugins/MarkerArray
           Enabled: false
@@ -411,7 +411,7 @@ Visualization Manager:
             Durability Policy: Volatile
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /marker
+            Value: marker
           Value: false
         - Alpha: 1
           Class: rviz_default_plugins/Polygon
@@ -424,7 +424,7 @@ Visualization Manager:
             Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /local_costmap/published_footprint
+            Value: local_costmap/published_footprint
           Value: true
         - Alpha: 1
           Autocompute Intensity Bounds: true
@@ -455,7 +455,7 @@ Visualization Manager:
             Durability Policy: Volatile
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /local_costmap/voxel_marked_cloud
+            Value: local_costmap/voxel_marked_cloud
           Use Fixed Frame: true
           Use rainbow: true
           Value: true
@@ -475,7 +475,7 @@ Visualization Manager:
             Durability Policy: Volatile
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /intel_realsense_r200_depth/image_raw
+            Value: intel_realsense_r200_depth/image_raw
           Value: true
         - Alpha: 1
           Autocompute Intensity Bounds: true
@@ -506,7 +506,7 @@ Visualization Manager:
             Durability Policy: Volatile
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /intel_realsense_r200_depth/points
+            Value: intel_realsense_r200_depth/points
           Use Fixed Frame: true
           Use rainbow: true
           Value: true
@@ -522,7 +522,7 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /waypoints
+        Value: waypoints
       Value: true
   Enabled: true
   Global Options:
@@ -545,15 +545,15 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /initialpose
+        Value: initialpose
     - Class: rviz_default_plugins/PublishPoint
       Single click: true
       Topic:
         Depth: 5
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /clicked_point
+        Value: clicked_point
     - Class: nav2_rviz_plugins/GoalTool
   Transformation:
     Current:

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -189,6 +189,17 @@ void ObstacleLayer::onInitialize()
     node->get_parameter(name_ + "." + source + "." + "raytrace_min_range", raytrace_min_range);
     node->get_parameter(name_ + "." + source + "." + "raytrace_max_range", raytrace_max_range);
 
+    // If the user passed a relative topic, use it as relative to this node's parent namespace.
+    // For example:
+    //   * User chosen namespace is `tb4`.
+    //   * User chosen topic is `scan`.
+    //   * Node namespace will be `/tb4/global_costmap`.
+    //   * Topic will be remapped to `/tb4/scan` rather than `/tb4/global_costmap/scan`
+    if (topic[0] != '/') {
+      std::string node_namespace = node->get_namespace();
+      std::string parent_namespace = node_namespace.substr(0, node_namespace.rfind("/"));
+      topic = parent_namespace + "/" + topic;
+    }
 
     RCLCPP_DEBUG(
       logger_,