diff --git a/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py b/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py
index 99ccb3caa46ab5f4c3e41dae6e994cdbd0097616..ef548f51926fede936b244d1a679ee0bca3a6897 100644
--- a/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py
+++ b/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py
@@ -21,6 +21,8 @@ import os
from ament_index_python.packages import get_package_prefix
from ament_index_python.packages import get_package_share_directory
+from nav2_common.launch import ReplaceString
+
from launch import LaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription,
@@ -47,6 +49,7 @@ def generate_launch_description():
map_yaml_file = LaunchConfiguration('map')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
+ rviz_config_file = LaunchConfiguration('rviz_config')
log_settings = LaunchConfiguration('log_settings', default='true')
# Declare the launch arguments
@@ -77,6 +80,11 @@ def generate_launch_description():
'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use')
+ declare_rviz_config_file_cmd = DeclareLaunchArgument(
+ 'rviz_config',
+ default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'),
+ description='Full path to the RVIZ config 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],
@@ -99,6 +107,10 @@ def generate_launch_description():
# Define commands for launching the navigation instances
nav_instances_cmds = []
for robot in robots:
+ namespaced_rviz_config_file = ReplaceString(
+ source_file=rviz_config_file,
+ replacements={'<robot_namespace>': ('/' + robot['name'])})
+
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.
@@ -123,6 +135,7 @@ def generate_launch_description():
'bt_xml_file': bt_xml_file,
'autostart': 'False',
'use_remappings': 'True',
+ 'rviz_config_file': namespaced_rviz_config_file,
'use_simulator': 'False'}.items()),
LogInfo(
@@ -136,7 +149,10 @@ def generate_launch_description():
msg=[robot['name'], ' params yaml: ', params_file]),
LogInfo(
condition=IfCondition(log_settings),
- msg=[robot['name'], ' behavior tree xml: ', bt_xml_file])
+ msg=[robot['name'], ' behavior tree xml: ', bt_xml_file]),
+ LogInfo(
+ condition=IfCondition(log_settings),
+ msg=[robot['name'], ' rviz config file: ', namespaced_rviz_config_file])
])
nav_instances_cmds.append(group)
@@ -158,6 +174,7 @@ def generate_launch_description():
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
+ ld.add_action(declare_rviz_config_file_cmd)
# Add the actions to start gazebo, robots and simulations
ld.add_action(start_gazebo_cmd)
diff --git a/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..161578a988398e4fecfe62e31ac79d260d463e5d
--- /dev/null
+++ b/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz
@@ -0,0 +1,349 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 195
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /TF1/Frames1
+ - /TF1/Tree1
+ - /Global Planner1/Global Costmap1
+ Splitter Ratio: 0.5833333134651184
+ Tree Height: 464
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded:
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: nav2_rviz_plugins/Navigation 2
+ Name: Navigation 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/RobotModel
+ Collision Enabled: false
+ Description File: ""
+ Description Source: Topic
+ Description Topic: <robot_namespace>/robot_description
+ Enabled: false
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ Name: RobotModel
+ TF Prefix: ""
+ Unreliable: false
+ Update Interval: 0
+ Value: false
+ Visual Enabled: true
+ - Class: rviz_default_plugins/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ base_footprint:
+ Value: true
+ base_link:
+ Value: true
+ base_scan:
+ Value: true
+ camera_depth_frame:
+ Value: true
+ camera_depth_optical_frame:
+ Value: true
+ camera_link:
+ Value: true
+ camera_rgb_frame:
+ Value: true
+ camera_rgb_optical_frame:
+ Value: true
+ caster_back_left_link:
+ Value: true
+ caster_back_right_link:
+ Value: true
+ imu_link:
+ Value: true
+ wheel_left_link:
+ Value: true
+ wheel_right_link:
+ Value: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: false
+ Tree:
+ {}
+ Update Interval: 0
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz_default_plugins/LaserScan
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 0
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: LaserScan
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic: <robot_namespace>/scan
+ Unreliable: true
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz_default_plugins/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: ""
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: Bumper Hit
+ Position Transformer: ""
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.07999999821186066
+ Style: Spheres
+ Topic: <robot_namespace>/mobile_base/sensors/bumper_pointcloud
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/Map
+ Color Scheme: map
+ Draw Behind: true
+ Enabled: true
+ Name: Map
+ Topic: <robot_namespace>/map
+ Unreliable: false
+ Use Timestamp: false
+ Value: true
+ - Alpha: 1
+ Arrow Length: 0.019999999552965164
+ Axes Length: 0.30000001192092896
+ Axes Radius: 0.009999999776482582
+ Class: rviz_default_plugins/PoseArray
+ Color: 0; 180; 0
+ Enabled: true
+ Head Length: 0.07000000029802322
+ Head Radius: 0.029999999329447746
+ Name: Amcl Particle Swarm
+ Shaft Length: 0.23000000417232513
+ Shaft Radius: 0.009999999776482582
+ Shape: Arrow (Flat)
+ Topic: <robot_namespace>/particlecloud
+ Unreliable: true
+ Value: true
+ - Class: rviz_common/Group
+ Displays:
+ - Alpha: 0.30000001192092896
+ Class: rviz_default_plugins/Map
+ Color Scheme: costmap
+ Draw Behind: false
+ Enabled: true
+ Name: Global Costmap
+ Topic: <robot_namespace>/global_costmap/costmap
+ Unreliable: false
+ Use Timestamp: false
+ Value: false
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz_default_plugins/Path
+ Color: 255; 0; 0
+ Enabled: true
+ Head Diameter: 0.019999999552965164
+ Head Length: 0.019999999552965164
+ Length: 0.30000001192092896
+ Line Style: Lines
+ Line Width: 0.029999999329447746
+ Name: Path
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: Arrows
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.004999999888241291
+ Shaft Length: 0.019999999552965164
+ Topic: <robot_namespace>/plan
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/Polygon
+ Color: 25; 255; 0
+ Enabled: false
+ Name: Polygon
+ Topic: <robot_namespace>/global_costmap/published_footprint
+ Unreliable: false
+ Value: false
+ Enabled: true
+ Name: Global Planner
+ - Class: rviz_common/Group
+ Displays:
+ - Alpha: 0.699999988079071
+ Class: rviz_default_plugins/Map
+ Color Scheme: costmap
+ Draw Behind: false
+ Enabled: true
+ Name: Local Costmap
+ Topic: <robot_namespace>/local_costmap/costmap
+ Unreliable: false
+ Use Timestamp: false
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz_default_plugins/Path
+ Color: 0; 12; 255
+ Enabled: true
+ Head Diameter: 0.30000001192092896
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
+ Line Style: Lines
+ Line Width: 0.029999999329447746
+ Name: Local Plan
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic: <robot_namespace>/local_plan
+ Unreliable: false
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: Trajectories
+ Namespaces:
+ {}
+ Queue Size: 10
+ Topic: <robot_namespace>/marker
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/Polygon
+ Color: 25; 255; 0
+ Enabled: true
+ Name: Polygon
+ Topic: <robot_namespace>/local_costmap/published_footprint
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Name: Local Planner
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Topic: <robot_namespace>/initialpose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic: <robot_namespace>/clicked_point
+ - Class: nav2_rviz_plugins/GoalTool
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Angle: -1.5700000524520874
+ Class: rviz_default_plugins/TopDownOrtho
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Scale: 134.638427734375
+ Target Frame: <Fixed Frame>
+ Value: TopDownOrtho (rviz_default_plugins)
+ X: -0.032615214586257935
+ Y: -0.0801941454410553
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 914
+ Hide Left Dock: false
+ Hide Right Dock: true
+ Navigation 2:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320100000313000000620000004300ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1545
+ X: 295
+ Y: 238
diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py
index 9beac57fb04d3655c677f9051c37eb50cf84a0b5..82764712d088abc82d282abfb6b4af42a2a626d8 100644
--- a/nav2_common/nav2_common/launch/__init__.py
+++ b/nav2_common/nav2_common/launch/__init__.py
@@ -13,4 +13,5 @@
# limitations under the License.
from .rewritten_yaml import RewrittenYaml
+from .replace_string import ReplaceString
from .node import Node
diff --git a/nav2_common/nav2_common/launch/replace_string.py b/nav2_common/nav2_common/launch/replace_string.py
new file mode 100644
index 0000000000000000000000000000000000000000..308b2fb96fe441cb685051a057dc3279a8f13b4b
--- /dev/null
+++ b/nav2_common/nav2_common/launch/replace_string.py
@@ -0,0 +1,73 @@
+# Copyright (c) 2019 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.
+
+from typing import Dict
+from typing import List
+from typing import Text
+import tempfile
+import launch
+
+class ReplaceString(launch.Substitution):
+ """
+ Substitution that replaces strings on a given file.
+ """
+
+ def __init__(self,
+ source_file: launch.SomeSubstitutionsType,
+ replacements: Dict) -> None:
+ super().__init__()
+
+ from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop
+ self.__source_file = normalize_to_list_of_substitutions(source_file)
+ self.__replacements = {}
+ for key in replacements:
+ self.__replacements[key] = normalize_to_list_of_substitutions(replacements[key])
+
+ @property
+ def name(self) -> List[launch.Substitution]:
+ """Getter for name."""
+ return self.__source_file
+
+ def describe(self) -> Text:
+ """Return a description of this substitution as a string."""
+ return ''
+
+ def perform(self, context: launch.LaunchContext) -> Text:
+ output_file = tempfile.NamedTemporaryFile(mode='w', delete=False)
+ replacements = self.resolve_replacements(context)
+ try:
+ input_file = open(launch.utilities.perform_substitutions(context, self.name), 'r')
+ self.replace(input_file, output_file, replacements)
+ except Exception as err:
+ print('ReplaceString substitution error: ', err)
+ finally:
+ input_file.close()
+ output_file.close()
+ return output_file.name
+
+ def resolve_replacements(self, context):
+ resolved_replacements = {}
+ for key in self.__replacements:
+ resolved_replacements[key] = launch.utilities.perform_substitutions(context, self.__replacements[key])
+ return resolved_replacements
+
+ def replace(self, input_file, output_file, replacements):
+ for line in input_file:
+ for key, value in replacements.items():
+ if isinstance(key, str) and isinstance(value, str):
+ if key in line:
+ line = line.replace(key, value)
+ else:
+ raise TypeError('A provided replacement pair is not a string. Both key and value should be strings.')
+ output_file.write(line)