Select Git revision
nav2_navigation_launch.py
nav2_navigation_launch.py 4.48 KiB
# Copyright (c) 2018 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.
import os
from ament_index_python.packages import get_package_prefix, get_package_share_directory
from nav2_common.launch import RewrittenYaml
from launch import LaunchDescription
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch_ros.actions import Node
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
use_sim_time = LaunchConfiguration('use_sim_time')
autostart = LaunchConfiguration('autostart')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
use_lifecycle_mgr = LaunchConfiguration('use_lifecycle_mgr')
map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')
# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time,
'bt_xml_filename': bt_xml_file,
'autostart': autostart,
'map_subscribe_transient_local': map_subscribe_transient_local}
configured_params = RewrittenYaml(
source_file=params_file,
rewrites=param_substitutions,
convert_types=True)
return LaunchDescription([
# Set env var to print messages to stdout immediately
SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
DeclareLaunchArgument(
'use_sim_time', default_value='false',
description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument(
'autostart', default_value='true',
description='Automatically startup the nav2 stack'),
DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
description='Full path to the ROS2 parameters file to use'),
DeclareLaunchArgument(
'bt_xml_file',
default_value=os.path.join(get_package_prefix('nav2_bt_navigator'),
'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use'),
DeclareLaunchArgument(
'use_lifecycle_mgr', default_value='true',
description='Whether to launch the lifecycle manager'),
DeclareLaunchArgument(
'map_subscribe_transient_local', default_value='false',
description='Whether to set the map subscriber QoS to transient local'),
Node(
package='nav2_controller',
node_executable='controller_server',
output='screen',
parameters=[configured_params]),
Node(
package='nav2_planner',
node_executable='planner_server',
node_name='planner_server',
output='screen',
parameters=[configured_params]),
Node(
package='nav2_recoveries',
node_executable='recoveries_node',
node_name='recoveries',
output='screen',
parameters=[{'use_sim_time': use_sim_time}]),
Node(
package='nav2_bt_navigator',
node_executable='bt_navigator',
node_name='bt_navigator',
output='screen',
parameters=[configured_params]),
Node(
condition=IfCondition(use_lifecycle_mgr),
package='nav2_lifecycle_manager',
node_executable='lifecycle_manager',
node_name='lifecycle_manager_navigation',
output='screen',
parameters=[{'use_sim_time': use_sim_time},
{'autostart': autostart},
{'node_names': ['controller_server',
'navfn_planner',
'bt_navigator']}]),
])