Skip to content
Snippets Groups Projects
Unverified Commit ec834a5d authored by Carlos A. Orduno's avatar Carlos A. Orduno Committed by GitHub
Browse files

[multirobot - Part4] Multiple robot control via RVIZ (#1178)

Adding functionality to launch a separate rviz instance per robot.
parent 8e886867
No related branches found
No related tags found
No related merge requests found
......@@ -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)
......
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
......@@ -13,4 +13,5 @@
# limitations under the License.
from .rewritten_yaml import RewrittenYaml
from .replace_string import ReplaceString
from .node import Node
# 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)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment