Skip to content

Commit 3f31112

Browse files
SteveMacenskiSteveMacenski
authored
Adding new Nav2 loopback simulator (#4614)
* adding Nav2 loopback sim Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * drop performance by half * lintin * Add multirobot usecase comment * fixing copy paste error * fixing review comments --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
1 parent 5321e00 commit 3f31112

File tree

16 files changed

+996
-2
lines changed

16 files changed

+996
-2
lines changed

‎nav2_bringup/launch/bringup_launch.py‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@ def generate_launch_description():
4848
use_composition = LaunchConfiguration('use_composition')
4949
use_respawn = LaunchConfiguration('use_respawn')
5050
log_level = LaunchConfiguration('log_level')
51+
use_localization = LaunchConfiguration('use_localization')
5152

5253
# Map fully qualified names to relative ones so the node's namespace can be prepended.
5354
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
@@ -99,6 +100,11 @@ def generate_launch_description():
99100
'map', default_value='', description='Full path to map yaml file to load'
100101
)
101102

103+
declare_use_localization_cmd = DeclareLaunchArgument(
104+
'use_localization', default_value='True',
105+
description='Whether to enable localization or not'
106+
)
107+
102108
declare_use_sim_time_cmd = DeclareLaunchArgument(
103109
'use_sim_time',
104110
default_value='false',
@@ -151,7 +157,7 @@ def generate_launch_description():
151157
PythonLaunchDescriptionSource(
152158
os.path.join(launch_dir, 'slam_launch.py')
153159
),
154-
condition=IfCondition(slam),
160+
condition=IfCondition(PythonExpression([slam, ' and ', use_localization])),
155161
launch_arguments={
156162
'namespace': namespace,
157163
'use_sim_time': use_sim_time,
@@ -164,7 +170,7 @@ def generate_launch_description():
164170
PythonLaunchDescriptionSource(
165171
os.path.join(launch_dir, 'localization_launch.py')
166172
),
167-
condition=IfCondition(PythonExpression(['not ', slam])),
173+
condition=IfCondition(PythonExpression(['not ', slam, ' and ', use_localization])),
168174
launch_arguments={
169175
'namespace': namespace,
170176
'map': map_yaml_file,
@@ -210,6 +216,7 @@ def generate_launch_description():
210216
ld.add_action(declare_use_composition_cmd)
211217
ld.add_action(declare_use_respawn_cmd)
212218
ld.add_action(declare_log_level_cmd)
219+
ld.add_action(declare_use_localization_cmd)
213220

214221
# Add the actions to launch all of the navigation nodes
215222
ld.add_action(bringup_cmd_group)

‎nav2_bringup/launch/tb3_loopback_simulation.launch.py‎

Lines changed: 224 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,224 @@
1+
# Copyright (C) 2024 Open Navigation LLC
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+
"""This is all-in-one launch script intended for use by nav2 developers."""
16+
17+
import os
18+
19+
from ament_index_python.packages import get_package_share_directory
20+
21+
from launch import LaunchDescription
22+
from launch.actions import (
23+
DeclareLaunchArgument,
24+
GroupAction,
25+
IncludeLaunchDescription,
26+
)
27+
from launch.conditions import IfCondition
28+
from launch.launch_description_sources import PythonLaunchDescriptionSource
29+
from launch.substitutions import LaunchConfiguration
30+
from launch_ros.actions import Node, SetParameter
31+
from launch_ros.descriptions import ParameterFile
32+
33+
from nav2_common.launch import RewrittenYaml
34+
35+
36+
def generate_launch_description():
37+
# Get the launch directory
38+
bringup_dir = get_package_share_directory('nav2_bringup')
39+
loopback_sim_dir = get_package_share_directory('nav2_loopback_sim')
40+
launch_dir = os.path.join(bringup_dir, 'launch')
41+
sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
42+
43+
# Create the launch configuration variables
44+
namespace = LaunchConfiguration('namespace')
45+
use_namespace = LaunchConfiguration('use_namespace')
46+
map_yaml_file = LaunchConfiguration('map')
47+
params_file = LaunchConfiguration('params_file')
48+
autostart = LaunchConfiguration('autostart')
49+
use_composition = LaunchConfiguration('use_composition')
50+
use_respawn = LaunchConfiguration('use_respawn')
51+
52+
# Launch configuration variables specific to simulation
53+
rviz_config_file = LaunchConfiguration('rviz_config_file')
54+
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
55+
use_rviz = LaunchConfiguration('use_rviz')
56+
57+
remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
58+
59+
# Declare the launch arguments
60+
declare_namespace_cmd = DeclareLaunchArgument(
61+
'namespace', default_value='', description='Top-level namespace'
62+
)
63+
64+
declare_use_namespace_cmd = DeclareLaunchArgument(
65+
'use_namespace',
66+
default_value='false',
67+
description='Whether to apply a namespace to the navigation stack',
68+
)
69+
70+
declare_map_yaml_cmd = DeclareLaunchArgument(
71+
'map',
72+
default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'),
73+
)
74+
75+
declare_params_file_cmd = DeclareLaunchArgument(
76+
'params_file',
77+
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
78+
description='Full path to the ROS2 parameters file to use for all launched nodes',
79+
)
80+
81+
declare_autostart_cmd = DeclareLaunchArgument(
82+
'autostart',
83+
default_value='true',
84+
description='Automatically startup the nav2 stack',
85+
)
86+
87+
declare_use_composition_cmd = DeclareLaunchArgument(
88+
'use_composition',
89+
default_value='True',
90+
description='Whether to use composed bringup',
91+
)
92+
93+
declare_use_respawn_cmd = DeclareLaunchArgument(
94+
'use_respawn',
95+
default_value='False',
96+
description='Whether to respawn if a node crashes. Applied when composition is disabled.',
97+
)
98+
99+
declare_rviz_config_file_cmd = DeclareLaunchArgument(
100+
'rviz_config_file',
101+
default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'),
102+
description='Full path to the RVIZ config file to use',
103+
)
104+
105+
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
106+
'use_robot_state_pub',
107+
default_value='True',
108+
description='Whether to start the robot state publisher',
109+
)
110+
111+
declare_use_rviz_cmd = DeclareLaunchArgument(
112+
'use_rviz', default_value='True', description='Whether to start RVIZ'
113+
)
114+
115+
urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
116+
with open(urdf, 'r') as infp:
117+
robot_description = infp.read()
118+
119+
start_robot_state_publisher_cmd = Node(
120+
condition=IfCondition(use_robot_state_pub),
121+
package='robot_state_publisher',
122+
executable='robot_state_publisher',
123+
name='robot_state_publisher',
124+
namespace=namespace,
125+
output='screen',
126+
parameters=[
127+
{'use_sim_time': True, 'robot_description': robot_description}
128+
],
129+
remappings=remappings,
130+
)
131+
132+
rviz_cmd = IncludeLaunchDescription(
133+
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')),
134+
condition=IfCondition(use_rviz),
135+
launch_arguments={
136+
'namespace': namespace,
137+
'use_namespace': use_namespace,
138+
'use_sim_time': 'True',
139+
'rviz_config': rviz_config_file,
140+
}.items(),
141+
)
142+
143+
bringup_cmd = IncludeLaunchDescription(
144+
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')),
145+
launch_arguments={
146+
'namespace': namespace,
147+
'use_namespace': use_namespace,
148+
'map': map_yaml_file,
149+
'use_sim_time': 'True',
150+
'params_file': params_file,
151+
'autostart': autostart,
152+
'use_composition': use_composition,
153+
'use_respawn': use_respawn,
154+
'use_localization': 'False', # Don't use SLAM, AMCL
155+
}.items(),
156+
)
157+
158+
loopback_sim_cmd = IncludeLaunchDescription(
159+
PythonLaunchDescriptionSource(
160+
os.path.join(loopback_sim_dir, 'loopback_simulation.launch.py')),
161+
launch_arguments={
162+
'params_file': params_file,
163+
}.items(),
164+
)
165+
166+
configured_params = ParameterFile(
167+
RewrittenYaml(
168+
source_file=params_file,
169+
root_key=namespace,
170+
param_rewrites={},
171+
convert_types=True,
172+
),
173+
allow_substs=True,
174+
)
175+
176+
start_map_server = GroupAction(
177+
actions=[
178+
SetParameter('use_sim_time', True),
179+
Node(
180+
package='nav2_map_server',
181+
executable='map_server',
182+
name='map_server',
183+
output='screen',
184+
respawn=use_respawn,
185+
respawn_delay=2.0,
186+
parameters=[configured_params, {'yaml_filename': map_yaml_file}],
187+
remappings=remappings,
188+
),
189+
Node(
190+
package='nav2_lifecycle_manager',
191+
executable='lifecycle_manager',
192+
name='lifecycle_manager_map_server',
193+
output='screen',
194+
parameters=[
195+
configured_params,
196+
{'autostart': autostart}, {'node_names': ['map_server']}],
197+
),
198+
]
199+
)
200+
201+
# Create the launch description and populate
202+
ld = LaunchDescription()
203+
204+
# Declare the launch options
205+
ld.add_action(declare_namespace_cmd)
206+
ld.add_action(declare_use_namespace_cmd)
207+
ld.add_action(declare_map_yaml_cmd)
208+
ld.add_action(declare_params_file_cmd)
209+
ld.add_action(declare_autostart_cmd)
210+
ld.add_action(declare_use_composition_cmd)
211+
212+
ld.add_action(declare_rviz_config_file_cmd)
213+
ld.add_action(declare_use_robot_state_pub_cmd)
214+
ld.add_action(declare_use_rviz_cmd)
215+
ld.add_action(declare_use_respawn_cmd)
216+
217+
# Add the actions to launch all of the navigation nodes
218+
ld.add_action(start_robot_state_publisher_cmd)
219+
ld.add_action(start_map_server)
220+
ld.add_action(loopback_sim_cmd)
221+
ld.add_action(rviz_cmd)
222+
ld.add_action(bringup_cmd)
223+
224+
return ld

0 commit comments

Comments
 (0)