Skip to content

Commit 7703c81

Browse files
author
Tony Najjar
authored
use_sim_time refactoring (ros-navigation#3131)
* initial commit * fix * fixes * revert * fix * linting * fixes * fix format * fixing pycodestyle * revert typo * add use_sim_time to costmap * add parameter description * fix defaults * fix formatting
1 parent 1c74368 commit 7703c81

23 files changed

Lines changed: 141 additions & 290 deletions

File tree

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,9 @@ bool BtActionServer<ActionT>::on_configure()
8282
"-r",
8383
std::string("__node:=") +
8484
std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node",
85+
"-p",
86+
"use_sim_time:=" +
87+
std::string(node->get_parameter("use_sim_time").as_bool() ? "true" : "false"),
8588
"--"});
8689

8790
// Support for handling the topic-based goal pose from rviz

‎nav2_bringup/launch/bringup_launch.py‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,6 @@ def generate_launch_description():
5555

5656
# Create our own temporary YAML files that include substitutions
5757
param_substitutions = {
58-
'use_sim_time': use_sim_time,
5958
'yaml_filename': map_yaml_file}
6059

6160
configured_params = RewrittenYaml(

‎nav2_bringup/launch/localization_launch.py‎

Lines changed: 30 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
2121
from launch.conditions import IfCondition
2222
from launch.substitutions import LaunchConfiguration, PythonExpression
23-
from launch_ros.actions import LoadComposableNodes
23+
from launch_ros.actions import LoadComposableNodes, SetParameter
2424
from launch_ros.actions import Node
2525
from launch_ros.descriptions import ComposableNode
2626
from nav2_common.launch import RewrittenYaml
@@ -53,7 +53,6 @@ def generate_launch_description():
5353

5454
# Create our own temporary YAML files that include substitutions
5555
param_substitutions = {
56-
'use_sim_time': use_sim_time,
5756
'yaml_filename': map_yaml_file}
5857

5958
configured_params = RewrittenYaml(
@@ -107,6 +106,7 @@ def generate_launch_description():
107106
load_nodes = GroupAction(
108107
condition=IfCondition(PythonExpression(['not ', use_composition])),
109108
actions=[
109+
SetParameter("use_sim_time", use_sim_time),
110110
Node(
111111
package='nav2_map_server',
112112
executable='map_server',
@@ -133,36 +133,39 @@ def generate_launch_description():
133133
name='lifecycle_manager_localization',
134134
output='screen',
135135
arguments=['--ros-args', '--log-level', log_level],
136-
parameters=[{'use_sim_time': use_sim_time},
137-
{'autostart': autostart},
136+
parameters=[{'autostart': autostart},
138137
{'node_names': lifecycle_nodes}])
139138
]
140139
)
141140

142-
load_composable_nodes = LoadComposableNodes(
141+
load_composable_nodes = GroupAction(
143142
condition=IfCondition(use_composition),
144-
target_container=container_name,
145-
composable_node_descriptions=[
146-
ComposableNode(
147-
package='nav2_map_server',
148-
plugin='nav2_map_server::MapServer',
149-
name='map_server',
150-
parameters=[configured_params],
151-
remappings=remappings),
152-
ComposableNode(
153-
package='nav2_amcl',
154-
plugin='nav2_amcl::AmclNode',
155-
name='amcl',
156-
parameters=[configured_params],
157-
remappings=remappings),
158-
ComposableNode(
159-
package='nav2_lifecycle_manager',
160-
plugin='nav2_lifecycle_manager::LifecycleManager',
161-
name='lifecycle_manager_localization',
162-
parameters=[{'use_sim_time': use_sim_time,
163-
'autostart': autostart,
164-
'node_names': lifecycle_nodes}]),
165-
],
143+
actions=[
144+
SetParameter("use_sim_time", use_sim_time),
145+
LoadComposableNodes(
146+
target_container=container_name,
147+
composable_node_descriptions=[
148+
ComposableNode(
149+
package='nav2_map_server',
150+
plugin='nav2_map_server::MapServer',
151+
name='map_server',
152+
parameters=[configured_params],
153+
remappings=remappings),
154+
ComposableNode(
155+
package='nav2_amcl',
156+
plugin='nav2_amcl::AmclNode',
157+
name='amcl',
158+
parameters=[configured_params],
159+
remappings=remappings),
160+
ComposableNode(
161+
package='nav2_lifecycle_manager',
162+
plugin='nav2_lifecycle_manager::LifecycleManager',
163+
name='lifecycle_manager_localization',
164+
parameters=[{'autostart': autostart,
165+
'node_names': lifecycle_nodes}]),
166+
],
167+
)
168+
]
166169
)
167170

168171
# Create the launch description and populate

‎nav2_bringup/launch/multi_tb3_simulation_launch.py‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -116,10 +116,9 @@ def generate_launch_description():
116116
group = GroupAction([
117117
IncludeLaunchDescription(
118118
PythonLaunchDescriptionSource(
119-
os.path.join(launch_dir, 'rviz_launch.py')),
119+
os.path.join(launch_dir, 'rviz_launch.py')),
120120
condition=IfCondition(use_rviz),
121-
launch_arguments={
122-
'namespace': TextSubstitution(text=robot['name']),
121+
launch_arguments={'namespace': TextSubstitution(text=robot['name']),
123122
'use_namespace': 'True',
124123
'rviz_config': rviz_config_file}.items()),
125124

‎nav2_bringup/launch/navigation_launch.py‎

Lines changed: 65 additions & 62 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
2121
from launch.conditions import IfCondition
2222
from launch.substitutions import LaunchConfiguration, PythonExpression
23-
from launch_ros.actions import LoadComposableNodes
23+
from launch_ros.actions import LoadComposableNodes, SetParameter
2424
from launch_ros.actions import Node
2525
from launch_ros.descriptions import ComposableNode
2626
from nav2_common.launch import RewrittenYaml
@@ -58,14 +58,13 @@ def generate_launch_description():
5858

5959
# Create our own temporary YAML files that include substitutions
6060
param_substitutions = {
61-
'use_sim_time': use_sim_time,
6261
'autostart': autostart}
6362

6463
configured_params = RewrittenYaml(
65-
source_file=params_file,
66-
root_key=namespace,
67-
param_rewrites=param_substitutions,
68-
convert_types=True)
64+
source_file=params_file,
65+
root_key=namespace,
66+
param_rewrites=param_substitutions,
67+
convert_types=True)
6968

7069
stdout_linebuf_envvar = SetEnvironmentVariable(
7170
'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
@@ -108,6 +107,7 @@ def generate_launch_description():
108107
load_nodes = GroupAction(
109108
condition=IfCondition(PythonExpression(['not ', use_composition])),
110109
actions=[
110+
SetParameter("use_sim_time", use_sim_time),
111111
Node(
112112
package='nav2_controller',
113113
executable='controller_server',
@@ -184,67 +184,70 @@ def generate_launch_description():
184184
name='lifecycle_manager_navigation',
185185
output='screen',
186186
arguments=['--ros-args', '--log-level', log_level],
187-
parameters=[{'use_sim_time': use_sim_time},
188-
{'autostart': autostart},
187+
parameters=[{'autostart': autostart},
189188
{'node_names': lifecycle_nodes}]),
190189
]
191190
)
192191

193-
load_composable_nodes = LoadComposableNodes(
192+
load_composable_nodes = GroupAction(
194193
condition=IfCondition(use_composition),
195-
target_container=container_name,
196-
composable_node_descriptions=[
197-
ComposableNode(
198-
package='nav2_controller',
199-
plugin='nav2_controller::ControllerServer',
200-
name='controller_server',
201-
parameters=[configured_params],
202-
remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
203-
ComposableNode(
204-
package='nav2_smoother',
205-
plugin='nav2_smoother::SmootherServer',
206-
name='smoother_server',
207-
parameters=[configured_params],
208-
remappings=remappings),
209-
ComposableNode(
210-
package='nav2_planner',
211-
plugin='nav2_planner::PlannerServer',
212-
name='planner_server',
213-
parameters=[configured_params],
214-
remappings=remappings),
215-
ComposableNode(
216-
package='nav2_behaviors',
217-
plugin='behavior_server::BehaviorServer',
218-
name='behavior_server',
219-
parameters=[configured_params],
220-
remappings=remappings),
221-
ComposableNode(
222-
package='nav2_bt_navigator',
223-
plugin='nav2_bt_navigator::BtNavigator',
224-
name='bt_navigator',
225-
parameters=[configured_params],
226-
remappings=remappings),
227-
ComposableNode(
228-
package='nav2_waypoint_follower',
229-
plugin='nav2_waypoint_follower::WaypointFollower',
230-
name='waypoint_follower',
231-
parameters=[configured_params],
232-
remappings=remappings),
233-
ComposableNode(
234-
package='nav2_velocity_smoother',
235-
plugin='nav2_velocity_smoother::VelocitySmoother',
236-
name='velocity_smoother',
237-
parameters=[configured_params],
238-
remappings=remappings +
239-
[('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
240-
ComposableNode(
241-
package='nav2_lifecycle_manager',
242-
plugin='nav2_lifecycle_manager::LifecycleManager',
243-
name='lifecycle_manager_navigation',
244-
parameters=[{'use_sim_time': use_sim_time,
245-
'autostart': autostart,
246-
'node_names': lifecycle_nodes}]),
247-
],
194+
actions=[
195+
SetParameter("use_sim_time", use_sim_time),
196+
LoadComposableNodes(
197+
target_container=container_name,
198+
composable_node_descriptions=[
199+
ComposableNode(
200+
package='nav2_controller',
201+
plugin='nav2_controller::ControllerServer',
202+
name='controller_server',
203+
parameters=[configured_params],
204+
remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
205+
ComposableNode(
206+
package='nav2_smoother',
207+
plugin='nav2_smoother::SmootherServer',
208+
name='smoother_server',
209+
parameters=[configured_params],
210+
remappings=remappings),
211+
ComposableNode(
212+
package='nav2_planner',
213+
plugin='nav2_planner::PlannerServer',
214+
name='planner_server',
215+
parameters=[configured_params],
216+
remappings=remappings),
217+
ComposableNode(
218+
package='nav2_behaviors',
219+
plugin='behavior_server::BehaviorServer',
220+
name='behavior_server',
221+
parameters=[configured_params],
222+
remappings=remappings),
223+
ComposableNode(
224+
package='nav2_bt_navigator',
225+
plugin='nav2_bt_navigator::BtNavigator',
226+
name='bt_navigator',
227+
parameters=[configured_params],
228+
remappings=remappings),
229+
ComposableNode(
230+
package='nav2_waypoint_follower',
231+
plugin='nav2_waypoint_follower::WaypointFollower',
232+
name='waypoint_follower',
233+
parameters=[configured_params],
234+
remappings=remappings),
235+
ComposableNode(
236+
package='nav2_velocity_smoother',
237+
plugin='nav2_velocity_smoother::VelocitySmoother',
238+
name='velocity_smoother',
239+
parameters=[configured_params],
240+
remappings=remappings +
241+
[('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
242+
ComposableNode(
243+
package='nav2_lifecycle_manager',
244+
plugin='nav2_lifecycle_manager::LifecycleManager',
245+
name='lifecycle_manager_navigation',
246+
parameters=[{'autostart': autostart,
247+
'node_names': lifecycle_nodes}]),
248+
],
249+
)
250+
]
248251
)
249252

250253
# Create the launch description and populate

‎nav2_bringup/launch/rviz_launch.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -61,8 +61,8 @@ def generate_launch_description():
6161
output='screen')
6262

6363
namespaced_rviz_config_file = ReplaceString(
64-
source_file=rviz_config_file,
65-
replacements={'<robot_namespace>': ('/', namespace)})
64+
source_file=rviz_config_file,
65+
replacements={'<robot_namespace>': ('/', namespace)})
6666

6767
start_namespaced_rviz_cmd = Node(
6868
condition=IfCondition(use_namespace),

‎nav2_bringup/launch/slam_launch.py‎

Lines changed: 24 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -17,11 +17,11 @@
1717
from ament_index_python.packages import get_package_share_directory
1818

1919
from launch import LaunchDescription
20-
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
20+
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
2121
from launch.conditions import IfCondition, UnlessCondition
2222
from launch.launch_description_sources import PythonLaunchDescriptionSource
2323
from launch.substitutions import LaunchConfiguration
24-
from launch_ros.actions import Node
24+
from launch_ros.actions import Node, SetParameter
2525
from nav2_common.launch import HasNodeParams, RewrittenYaml
2626

2727

@@ -43,13 +43,10 @@ def generate_launch_description():
4343
slam_launch_file = os.path.join(slam_toolbox_dir, 'launch', 'online_sync_launch.py')
4444

4545
# Create our own temporary YAML files that include substitutions
46-
param_substitutions = {
47-
'use_sim_time': use_sim_time}
48-
4946
configured_params = RewrittenYaml(
5047
source_file=params_file,
5148
root_key=namespace,
52-
param_rewrites=param_substitutions,
49+
param_rewrites={},
5350
convert_types=True)
5451

5552
# Declare the launch arguments
@@ -82,24 +79,26 @@ def generate_launch_description():
8279

8380
# Nodes launching commands
8481

85-
start_map_saver_server_cmd = Node(
86-
package='nav2_map_server',
87-
executable='map_saver_server',
88-
output='screen',
89-
respawn=use_respawn,
90-
respawn_delay=2.0,
91-
arguments=['--ros-args', '--log-level', log_level],
92-
parameters=[configured_params])
93-
94-
start_lifecycle_manager_cmd = Node(
95-
package='nav2_lifecycle_manager',
96-
executable='lifecycle_manager',
97-
name='lifecycle_manager_slam',
98-
output='screen',
99-
arguments=['--ros-args', '--log-level', log_level],
100-
parameters=[{'use_sim_time': use_sim_time},
101-
{'autostart': autostart},
102-
{'node_names': lifecycle_nodes}])
82+
start_map_server = GroupAction(
83+
actions=[
84+
SetParameter("use_sim_time", use_sim_time),
85+
Node(
86+
package='nav2_map_server',
87+
executable='map_saver_server',
88+
output='screen',
89+
respawn=use_respawn,
90+
respawn_delay=2.0,
91+
arguments=['--ros-args', '--log-level', log_level],
92+
parameters=[configured_params]),
93+
Node(
94+
package='nav2_lifecycle_manager',
95+
executable='lifecycle_manager',
96+
name='lifecycle_manager_slam',
97+
output='screen',
98+
arguments=['--ros-args', '--log-level', log_level],
99+
parameters=[{'autostart': autostart},
100+
{'node_names': lifecycle_nodes}])
101+
])
103102

104103
# If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file'
105104
# LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load
@@ -129,8 +128,7 @@ def generate_launch_description():
129128
ld.add_action(declare_log_level_cmd)
130129

131130
# Running Map Saver Server
132-
ld.add_action(start_map_saver_server_cmd)
133-
ld.add_action(start_lifecycle_manager_cmd)
131+
ld.add_action(start_map_server)
134132

135133
# Running SLAM Toolbox (Only one of them will be run)
136134
ld.add_action(start_slam_toolbox_cmd)

0 commit comments

Comments
 (0)