Skip to content

Commit 9b24655

Browse files
hyunseok-yanghyunseok-yang
committed
Add new luanch script for multi-robot bringup
Rename luanch script for multi-robot simulation bringup Add new nav2_common script - Parse argument - Parse multirobot pose
1 parent b8f8ed7 commit 9b24655

File tree

4 files changed

+256
-0
lines changed

4 files changed

+256
-0
lines changed

‎nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py‎

Lines changed: 173 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,173 @@
1+
# Copyright (c) 2023 LG Electronics.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
16+
import os
17+
from ament_index_python.packages import get_package_share_directory
18+
from launch import LaunchDescription
19+
from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction,
20+
IncludeLaunchDescription, LogInfo)
21+
from launch.conditions import IfCondition
22+
from launch.launch_description_sources import PythonLaunchDescriptionSource
23+
from launch.substitutions import LaunchConfiguration, TextSubstitution
24+
from nav2_common.launch import ParseMultiRobotPose
25+
26+
27+
def generate_launch_description():
28+
29+
# Get the launch directory
30+
bringup_dir = get_package_share_directory('nav2_bringup')
31+
launch_dir = os.path.join(bringup_dir, 'launch')
32+
33+
# Simulation settings
34+
world = LaunchConfiguration('world')
35+
simulator = LaunchConfiguration('simulator')
36+
37+
# On this example all robots are launched with the same settings
38+
map_yaml_file = LaunchConfiguration('map')
39+
params_file = LaunchConfiguration('params_file')
40+
autostart = LaunchConfiguration('autostart')
41+
rviz_config_file = LaunchConfiguration('rviz_config')
42+
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
43+
use_rviz = LaunchConfiguration('use_rviz')
44+
log_settings = LaunchConfiguration('log_settings', default='true')
45+
46+
# Declare the launch arguments
47+
declare_world_cmd = DeclareLaunchArgument(
48+
'world',
49+
default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'),
50+
description='Full path to world file to load')
51+
52+
declare_simulator_cmd = DeclareLaunchArgument(
53+
'simulator',
54+
default_value='gazebo',
55+
description='The simulator to use (gazebo or gzserver)')
56+
57+
declare_map_yaml_cmd = DeclareLaunchArgument(
58+
'map',
59+
default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'),
60+
description='Full path to map file to load')
61+
62+
declare_params_file_cmd = DeclareLaunchArgument(
63+
'params_file',
64+
default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_all.yaml'),
65+
description='Full path to the ROS2 parameters file to use for all launched nodes')
66+
67+
declare_autostart_cmd = DeclareLaunchArgument(
68+
'autostart', default_value='false',
69+
description='Automatically startup the stacks')
70+
71+
declare_rviz_config_file_cmd = DeclareLaunchArgument(
72+
'rviz_config',
73+
default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'),
74+
description='Full path to the RVIZ config file to use.')
75+
76+
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
77+
'use_robot_state_pub',
78+
default_value='True',
79+
description='Whether to start the robot state publisher')
80+
81+
declare_use_rviz_cmd = DeclareLaunchArgument(
82+
'use_rviz',
83+
default_value='True',
84+
description='Whether to start RVIZ')
85+
86+
# Start Gazebo with plugin providing the robot spawning service
87+
start_gazebo_cmd = ExecuteProcess(
88+
cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so',
89+
'-s', 'libgazebo_ros_factory.so', world],
90+
output='screen')
91+
92+
# To bringup multi-robot,
93+
# give arguments robot name(which is namespace) and pose for initialization
94+
# ex) robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}"
95+
# ex) robots:="robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707};
96+
# robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}"
97+
# Keep general yaml format
98+
99+
robots_list = ParseMultiRobotPose('robots').value()
100+
101+
# Define commands for launching the navigation instances
102+
bringup_cmd_group = []
103+
for robot_name in robots_list:
104+
init_pose = robots_list[robot_name]
105+
group = GroupAction([
106+
LogInfo(msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose)]),
107+
108+
IncludeLaunchDescription(
109+
PythonLaunchDescriptionSource(
110+
os.path.join(launch_dir, 'rviz_launch.py')),
111+
condition=IfCondition(use_rviz),
112+
launch_arguments={'namespace': TextSubstitution(text=robot_name),
113+
'use_namespace': 'True',
114+
'rviz_config': rviz_config_file}.items()),
115+
116+
IncludeLaunchDescription(
117+
PythonLaunchDescriptionSource(os.path.join(bringup_dir,
118+
'launch',
119+
'tb3_simulation_launch.py')),
120+
launch_arguments={'namespace': robot_name,
121+
'use_namespace': 'True',
122+
'map': map_yaml_file,
123+
'use_sim_time': 'True',
124+
'params_file': params_file,
125+
'autostart': autostart,
126+
'use_rviz': 'False',
127+
'use_simulator': 'False',
128+
'headless': 'False',
129+
'use_robot_state_pub': use_robot_state_pub,
130+
'x_pose': TextSubstitution(text=str(init_pose['x'])),
131+
'y_pose': TextSubstitution(text=str(init_pose['y'])),
132+
'z_pose': TextSubstitution(text=str(init_pose['z'])),
133+
'roll': TextSubstitution(text=str(init_pose['roll'])),
134+
'pitch': TextSubstitution(text=str(init_pose['pitch'])),
135+
'yaw': TextSubstitution(text=str(init_pose['yaw'])),
136+
'robot_name':TextSubstitution(text=robot_name), }.items())
137+
])
138+
139+
bringup_cmd_group.append(group)
140+
141+
# Create the launch description and populate
142+
ld = LaunchDescription()
143+
144+
# Declare the launch options
145+
ld.add_action(declare_simulator_cmd)
146+
ld.add_action(declare_world_cmd)
147+
ld.add_action(declare_map_yaml_cmd)
148+
ld.add_action(declare_params_file_cmd)
149+
ld.add_action(declare_use_rviz_cmd)
150+
ld.add_action(declare_autostart_cmd)
151+
ld.add_action(declare_rviz_config_file_cmd)
152+
ld.add_action(declare_use_robot_state_pub_cmd)
153+
154+
# Add the actions to start gazebo, robots and simulations
155+
ld.add_action(start_gazebo_cmd)
156+
157+
ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))]))
158+
159+
ld.add_action(LogInfo(condition=IfCondition(log_settings),
160+
msg=['map yaml: ', map_yaml_file]))
161+
ld.add_action(LogInfo(condition=IfCondition(log_settings),
162+
msg=['params yaml: ', params_file]))
163+
ld.add_action(LogInfo(condition=IfCondition(log_settings),
164+
msg=['rviz config file: ', rviz_config_file]))
165+
ld.add_action(LogInfo(condition=IfCondition(log_settings),
166+
msg=['using robot state pub: ', use_robot_state_pub]))
167+
ld.add_action(LogInfo(condition=IfCondition(log_settings),
168+
msg=['autostart: ', autostart]))
169+
170+
for cmd in bringup_cmd_group:
171+
ld.add_action(cmd)
172+
173+
return ld

nav2_bringup/launch/multi_tb3_simulation_launch.py renamed to nav2_bringup/launch/unique_multi_tb3_simulation_launch.py

File renamed without changes.

‎nav2_common/nav2_common/launch/__init__.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,3 +15,4 @@
1515
from .has_node_params import HasNodeParams
1616
from .rewritten_yaml import RewrittenYaml
1717
from .replace_string import ReplaceString
18+
from .parse_multirobot_pose import ParseMultiRobotPose

‎nav2_common/nav2_common/launch/parse_multirobot_pose.py‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
# Copyright (c) 2023 LG Electronics.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
import yaml
16+
import sys
17+
from typing import Text, Dict
18+
19+
20+
class ParseMultiRobotPose():
21+
"""
22+
Parsing argument using sys module
23+
"""
24+
25+
def __init__(self, target_argument: Text):
26+
"""
27+
Parse arguments for multi-robot's pose
28+
29+
for example,
30+
`ros2 launch nav2_bringup bringup_multirobot_launch.py
31+
robots:="robot1={x: 1.0, y: 1.0, yaw: 0.0};
32+
robot2={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}"`
33+
34+
`target_argument` shall be 'robots'.
35+
Then, this will parse a string value for `robots` argument.
36+
37+
Each robot name which is corresponding to namespace and pose of it will be separted by `;`.
38+
The pose consists of x, y and yaw with YAML format.
39+
40+
:param: target argument name to parse
41+
"""
42+
self.__args: Text = self.__parse_argument(target_argument)
43+
44+
def __parse_argument(self, target_argument: Text) -> Text:
45+
"""
46+
get value of target argument
47+
"""
48+
if len(sys.argv) > 4:
49+
argv = sys.argv[4:]
50+
for arg in argv:
51+
if arg.startswith(target_argument + ":="):
52+
return arg.replace(target_argument + ":=", "")
53+
return ""
54+
55+
def value(self) -> Dict:
56+
"""
57+
get value of target argument
58+
"""
59+
args = self.__args
60+
parsed_args = list() if len(args) == 0 else args.split(';')
61+
multirobots = dict()
62+
for arg in parsed_args:
63+
key_val = arg.strip().split('=')
64+
if len(key_val) != 2:
65+
continue
66+
key = key_val[0].strip()
67+
val = key_val[1].strip()
68+
robot_pose = yaml.safe_load(val)
69+
if 'x' not in robot_pose:
70+
robot_pose['x'] = 0.0
71+
if 'y' not in robot_pose:
72+
robot_pose['y'] = 0.0
73+
if 'z' not in robot_pose:
74+
robot_pose['z'] = 0.0
75+
if 'roll' not in robot_pose:
76+
robot_pose['roll'] = 0.0
77+
if 'pitch' not in robot_pose:
78+
robot_pose['pitch'] = 0.0
79+
if 'yaw' not in robot_pose:
80+
robot_pose['yaw'] = 0.0
81+
multirobots[key] = robot_pose
82+
return multirobots

0 commit comments

Comments
 (0)