diff --git a/nav2_bringup/bringup/launch/nav2_bringup_launch.py b/nav2_bringup/bringup/launch/nav2_bringup_launch.py
index 9ea91bc080..156f6b7c42 100644
--- a/nav2_bringup/bringup/launch/nav2_bringup_launch.py
+++ b/nav2_bringup/bringup/launch/nav2_bringup_launch.py
@@ -16,11 +16,12 @@
from ament_index_python.packages import get_package_prefix, get_package_share_directory
+from nav2_common.launch import Node
+
from launch import LaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, IncludeLaunchDescription
-from launch_ros.actions import Node
def generate_launch_description():
@@ -29,15 +30,32 @@ def generate_launch_description():
launch_dir = os.path.join(bringup_dir, 'launch')
# Create the launch configuration variables
+ namespace = LaunchConfiguration('namespace')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
autostart = LaunchConfiguration('autostart')
+ use_remappings = LaunchConfiguration('use_remappings')
+
+ # TODO(orduno) Remove once `PushNodeRemapping` is resolved
+ # https://github.com/ros2/launch_ros/issues/56
+ remappings = [((namespace, '/tf'), '/tf'),
+ ((namespace, '/tf_static'), '/tf_static'),
+ ('/scan', 'scan'),
+ ('/tf', 'tf'),
+ ('/tf_static', 'tf_static'),
+ ('/cmd_vel', 'cmd_vel'),
+ ('/map', 'map')]
stdout_linebuf_envvar = SetEnvironmentVariable(
'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
+ declare_namespace_cmd = DeclareLaunchArgument(
+ 'namespace',
+ default_value='',
+ description='Top-level namespace')
+
# Declare the launch arguments
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
@@ -64,22 +82,30 @@ def generate_launch_description():
'autostart', default_value='true',
description='Automatically startup the nav2 stack')
+ declare_use_remappings_cmd = DeclareLaunchArgument(
+ 'use_remappings', default_value='false',
+ description='Arguments to pass to all nodes launched by the file')
+
# Specify the actions
start_localization_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'nav2_localization_launch.py')),
- launch_arguments={'map': map_yaml_file,
+ launch_arguments={'namespace': namespace,
+ 'map': map_yaml_file,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
- 'use_lifecycle_mgr': 'false'}.items())
+ 'use_lifecycle_mgr': 'false',
+ 'use_remappings': use_remappings}.items())
start_navigation_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'nav2_navigation_launch.py')),
- launch_arguments={'use_sim_time': use_sim_time,
+ launch_arguments={'namespace': namespace,
+ 'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
'bt_xml_file': bt_xml_file,
'use_lifecycle_mgr': 'false',
+ 'use_remappings': use_remappings,
'map_subscribe_transient_local': 'true'}.items())
start_lifecycle_manager_cmd = Node(
@@ -102,11 +128,13 @@ def generate_launch_description():
ld.add_action(stdout_linebuf_envvar)
# Declare the launch options
+ ld.add_action(declare_namespace_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_bt_xml_cmd)
+ ld.add_action(declare_use_remappings_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(start_lifecycle_manager_cmd)
diff --git a/nav2_bringup/bringup/launch/nav2_localization_launch.py b/nav2_bringup/bringup/launch/nav2_localization_launch.py
index fd0b89ddbc..14855b6967 100644
--- a/nav2_bringup/bringup/launch/nav2_localization_launch.py
+++ b/nav2_bringup/bringup/launch/nav2_localization_launch.py
@@ -17,38 +17,58 @@
from ament_index_python.packages import get_package_share_directory
from nav2_common.launch import RewrittenYaml
+from nav2_common.launch import Node
from launch import LaunchDescription
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
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')
+ namespace = LaunchConfiguration('namespace')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
autostart = LaunchConfiguration('autostart')
params_file = LaunchConfiguration('params_file')
use_lifecycle_mgr = LaunchConfiguration('use_lifecycle_mgr')
+ use_remappings = LaunchConfiguration('use_remappings')
+
+ # TODO(orduno) Remove once `PushNodeRemapping` is resolved
+ # https://github.com/ros2/launch_ros/issues/56
+ remappings = [((namespace, '/tf'), '/tf'),
+ ((namespace, '/tf_static'), '/tf_static'),
+ ('/scan', 'scan'),
+ ('/tf', 'tf'),
+ ('/tf_static', 'tf_static'),
+ ('/cmd_vel', 'cmd_vel'),
+ ('/map', 'map'),
+ ('/goal_pose', 'goal_pose')]
# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time,
'yaml_filename': map_yaml_file}
+ namespace_substitutions = {'navigation_namespace': namespace}
+
configured_params = RewrittenYaml(
source_file=params_file,
- rewrites=param_substitutions,
+ param_rewrites=param_substitutions,
+ key_rewrites=namespace_substitutions,
convert_types=True)
return LaunchDescription([
# Set env var to print messages to stdout immediately
SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
+ DeclareLaunchArgument(
+ 'namespace', default_value='',
+ description='Top-level namespace'),
+
DeclareLaunchArgument(
'map',
default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'),
@@ -71,19 +91,27 @@ def generate_launch_description():
'use_lifecycle_mgr', default_value='true',
description='Whether to launch the lifecycle manager'),
+ DeclareLaunchArgument(
+ 'use_remappings', default_value='false',
+ description='Arguments to pass to all nodes launched by the file'),
+
Node(
package='nav2_map_server',
node_executable='map_server',
node_name='map_server',
output='screen',
- parameters=[configured_params]),
+ parameters=[configured_params],
+ use_remappings=IfCondition(use_remappings),
+ remappings=remappings),
Node(
package='nav2_amcl',
node_executable='amcl',
node_name='amcl',
output='screen',
- parameters=[configured_params]),
+ parameters=[configured_params],
+ use_remappings=IfCondition(use_remappings),
+ remappings=remappings),
Node(
condition=IfCondition(use_lifecycle_mgr),
diff --git a/nav2_bringup/bringup/launch/nav2_navigation_launch.py b/nav2_bringup/bringup/launch/nav2_navigation_launch.py
index 93b51de0db..be9210ccd2 100644
--- a/nav2_bringup/bringup/launch/nav2_navigation_launch.py
+++ b/nav2_bringup/bringup/launch/nav2_navigation_launch.py
@@ -17,25 +17,38 @@
from ament_index_python.packages import get_package_prefix, get_package_share_directory
from nav2_common.launch import RewrittenYaml
+from nav2_common.launch import Node
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')
+ namespace = LaunchConfiguration('namespace')
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')
+ use_remappings = LaunchConfiguration('use_remappings')
map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')
+ # TODO(orduno) Remove once `PushNodeRemapping` is resolved
+ # https://github.com/ros2/launch_ros/issues/56
+ remappings = [((namespace, '/tf'), '/tf'),
+ ((namespace, '/tf_static'), '/tf_static'),
+ ('/scan', 'scan'),
+ ('/tf', 'tf'),
+ ('/tf_static', 'tf_static'),
+ ('/cmd_vel', 'cmd_vel'),
+ ('/map', 'map'),
+ ('/goal_pose', 'goal_pose')]
+
# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time,
@@ -43,15 +56,22 @@ def generate_launch_description():
'autostart': autostart,
'map_subscribe_transient_local': map_subscribe_transient_local}
+ namespace_substitutions = {'navigation_namespace': namespace}
+
configured_params = RewrittenYaml(
source_file=params_file,
- rewrites=param_substitutions,
+ param_rewrites=param_substitutions,
+ key_rewrites=namespace_substitutions,
convert_types=True)
return LaunchDescription([
# Set env var to print messages to stdout immediately
SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
+ DeclareLaunchArgument(
+ 'namespace', default_value='',
+ description='Top-level namespace'),
+
DeclareLaunchArgument(
'use_sim_time', default_value='false',
description='Use simulation (Gazebo) clock if true'),
@@ -75,6 +95,10 @@ def generate_launch_description():
'use_lifecycle_mgr', default_value='true',
description='Whether to launch the lifecycle manager'),
+ DeclareLaunchArgument(
+ 'use_remappings', default_value='false',
+ description='Arguments to pass to all nodes launched by the file'),
+
DeclareLaunchArgument(
'map_subscribe_transient_local', default_value='false',
description='Whether to set the map subscriber QoS to transient local'),
@@ -83,28 +107,36 @@ def generate_launch_description():
package='nav2_controller',
node_executable='controller_server',
output='screen',
- parameters=[configured_params]),
+ parameters=[configured_params],
+ use_remappings=IfCondition(use_remappings),
+ remappings=remappings),
Node(
package='nav2_planner',
node_executable='planner_server',
node_name='planner_server',
output='screen',
- parameters=[configured_params]),
+ parameters=[configured_params],
+ use_remappings=IfCondition(use_remappings),
+ remappings=remappings),
Node(
package='nav2_recoveries',
node_executable='recoveries_node',
node_name='recoveries',
output='screen',
- parameters=[{'use_sim_time': use_sim_time}]),
+ parameters=[{'use_sim_time': use_sim_time}],
+ use_remappings=IfCondition(use_remappings),
+ remappings=remappings),
Node(
package='nav2_bt_navigator',
node_executable='bt_navigator',
node_name='bt_navigator',
output='screen',
- parameters=[configured_params]),
+ parameters=[configured_params],
+ use_remappings=IfCondition(use_remappings),
+ remappings=remappings),
Node(
condition=IfCondition(use_lifecycle_mgr),
diff --git a/nav2_bringup/bringup/launch/nav2_tb3_simulation_launch.py b/nav2_bringup/bringup/launch/nav2_tb3_simulation_launch.py
index 013a9ae443..45d18d8b85 100644
--- a/nav2_bringup/bringup/launch/nav2_tb3_simulation_launch.py
+++ b/nav2_bringup/bringup/launch/nav2_tb3_simulation_launch.py
@@ -18,15 +18,16 @@
from ament_index_python.packages import get_package_prefix, get_package_share_directory
+from nav2_common.launch import Node
+
from launch import LaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+from launch.substitutions import LaunchConfiguration
from launch.actions import (DeclareLaunchArgument, SetEnvironmentVariable,
IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler, EmitEvent)
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
-from launch_ros.actions import Node
def generate_launch_description():
@@ -35,11 +36,13 @@ def generate_launch_description():
launch_dir = os.path.join(bringup_dir, 'launch')
# Create the launch configuration variables
+ namespace = LaunchConfiguration('namespace')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
autostart = LaunchConfiguration('autostart')
+ use_remappings = LaunchConfiguration('use_remappings')
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
@@ -47,7 +50,22 @@ def generate_launch_description():
simulator = LaunchConfiguration('simulator')
world = LaunchConfiguration('world')
+ # TODO(orduno) Remove once `PushNodeRemapping` is resolved
+ # https://github.com/ros2/launch_ros/issues/56
+ remappings = [((namespace, '/tf'), '/tf'),
+ ((namespace, '/tf_static'), '/tf_static'),
+ ('/scan', 'scan'),
+ ('/tf', 'tf'),
+ ('/tf_static', 'tf_static'),
+ ('/cmd_vel', 'cmd_vel'),
+ ('/map', 'map')]
+
# Declare the launch arguments
+ declare_namespace_cmd = DeclareLaunchArgument(
+ 'namespace',
+ default_value='',
+ description='Top-level namespace')
+
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'),
@@ -74,6 +92,10 @@ def generate_launch_description():
'autostart', default_value='false',
description='Automatically startup the nav2 stack')
+ declare_use_remappings_cmd = DeclareLaunchArgument(
+ 'use_remappings', default_value='false',
+ description='Arguments to pass to all nodes launched by the file')
+
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config_file',
default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'),
@@ -113,20 +135,39 @@ def generate_launch_description():
node_name='robot_state_publisher',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
+ use_remappings=IfCondition(use_remappings),
+ remappings=remappings,
arguments=[urdf])
# TODO(orduno) RVIZ crashing if launched as a node: https://github.com/ros2/rviz/issues/442
# Launching as node works after applying the change described on the github issue.
+ # Once fixed, launch by providing the remappings:
+ # rviz_remappings = [('/tf', 'tf'),
+ # ('/tf_static', 'tf_static'),
+ # ('goal_pose', 'goal_pose'),
+ # ('/clicked_point', 'clicked_point'),
+ # ('/initialpose', 'initialpose'),
+ # ('/parameter_events', 'parameter_events'),
+ # ('/rosout', 'rosout')]
+
# start_rviz_cmd = Node(
# package='rviz2',
# node_executable='rviz2',
# node_name='rviz2',
# arguments=['-d', rviz_config_file],
- # output='screen')
+ # output='screen',
+ # use_remappings=IfCondition(use_remappings),
+ # remappings=rviz_remappings)
start_rviz_cmd = ExecuteProcess(
cmd=[os.path.join(get_package_prefix('rviz2'), 'lib/rviz2/rviz2'),
- ['-d', rviz_config_file]],
+ ['-d', rviz_config_file],
+ ['__ns:=/', namespace],
+ '/tf:=tf',
+ '/tf_static:=tf_static',
+ '/goal_pose:=goal_pose',
+ '/clicked_point:=clicked_point',
+ '/initialpose:=initialpose'],
cwd=[launch_dir], output='screen')
exit_event_handler = RegisterEventHandler(
@@ -136,21 +177,25 @@ def generate_launch_description():
bringup_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'nav2_bringup_launch.py')),
- launch_arguments={'map': map_yaml_file,
+ launch_arguments={'namespace': namespace,
+ 'map': map_yaml_file,
'use_sim_time': use_sim_time,
'params_file': params_file,
'bt_xml_file': bt_xml_file,
- 'autostart': autostart}.items())
+ 'autostart': autostart,
+ 'use_remappings': use_remappings}.items())
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
+ ld.add_action(declare_namespace_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
ld.add_action(declare_autostart_cmd)
+ ld.add_action(declare_use_remappings_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_simulator_cmd)
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
index 84ba49c0f0..35a624f0d1 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
@@ -15,8 +15,8 @@
-
-
+
+
diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py
index b4ad85d1ab..9beac57fb0 100644
--- a/nav2_common/nav2_common/launch/__init__.py
+++ b/nav2_common/nav2_common/launch/__init__.py
@@ -13,3 +13,4 @@
# limitations under the License.
from .rewritten_yaml import RewrittenYaml
+from .node import Node
diff --git a/nav2_common/nav2_common/launch/node.py b/nav2_common/nav2_common/launch/node.py
new file mode 100644
index 0000000000..b27c035e72
--- /dev/null
+++ b/nav2_common/nav2_common/launch/node.py
@@ -0,0 +1,385 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+# 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.
+
+"""Module for the Node action."""
+
+import os
+import pathlib
+from tempfile import NamedTemporaryFile
+from typing import cast
+from typing import Dict
+from typing import Iterable
+from typing import List
+from typing import Optional
+from typing import Text # noqa: F401
+from typing import Tuple # noqa: F401
+from typing import Union
+
+from launch import Condition
+from launch.action import Action
+from launch.actions import ExecuteProcess
+from launch.frontend import Entity
+# TODO(orduno) See comment below
+# from launch.frontend import expose_action
+from launch.frontend import Parser
+from launch.launch_context import LaunchContext
+import launch.logging
+from launch.some_substitutions_type import SomeSubstitutionsType
+from launch.substitutions import LocalSubstitution
+from launch.substitutions import TextSubstitution
+from launch.utilities import ensure_argument_type
+from launch.utilities import normalize_to_list_of_substitutions
+from launch.utilities import perform_substitutions
+
+from launch_ros.parameters_type import SomeParameters
+from launch_ros.remap_rule_type import SomeRemapRules
+from launch_ros.substitutions import ExecutableInPackage
+from launch_ros.utilities import evaluate_parameters
+from launch_ros.utilities import normalize_parameters
+from launch_ros.utilities import normalize_remap_rules
+
+from rclpy.validate_namespace import validate_namespace
+from rclpy.validate_node_name import validate_node_name
+
+import yaml
+
+
+#TODO(orduno) Figure out how to import this decorator correctly
+# @expose_action('node')
+class Node(ExecuteProcess):
+ """Action that executes a ROS node."""
+
+ def __init__(
+ self, *,
+ package: SomeSubstitutionsType,
+ node_executable: SomeSubstitutionsType,
+ node_name: Optional[SomeSubstitutionsType] = None,
+ node_namespace: SomeSubstitutionsType = '',
+ parameters: Optional[SomeParameters] = None,
+ use_remappings: Optional[Condition] = None,
+ remappings: Optional[SomeRemapRules] = None,
+ arguments: Optional[Iterable[SomeSubstitutionsType]] = None,
+ **kwargs
+ ) -> None:
+ """
+ Construct an Node action.
+
+ Many arguments are passed eventually to
+ :class:`launch.actions.ExecuteProcess`, so see the documentation of
+ that class for additional details.
+ However, the `cmd` is not meant to be used, instead use the
+ `node_executable` and `arguments` keyword arguments to this function.
+
+ This action, once executed, delegates most work to the
+ :class:`launch.actions.ExecuteProcess`, but it also converts some ROS
+ specific arguments into generic command line arguments.
+
+ The launch_ros.substitutions.ExecutableInPackage substitution is used
+ to find the executable at runtime, so this Action also raise the
+ exceptions that substituion can raise when the package or executable
+ are not found.
+
+ If the node_name is not given (or is None) then no name is passed to
+ the node on creation and instead the default name specified within the
+ code of the node is used instead.
+
+ The node_namespace can either be absolute (i.e. starts with /) or
+ relative.
+ If absolute, then nothing else is considered and this is passed
+ directly to the node to set the namespace.
+ If relative, the namespace in the 'ros_namespace' LaunchConfiguration
+ will be prepended to the given relative node namespace.
+ If no node_namespace is given, then the default namespace `/` is
+ assumed.
+
+ The parameters are passed as a list, with each element either a yaml
+ file that contains parameter rules (string or pathlib.Path to the full
+ path of the file), or a dictionary that specifies parameter rules.
+ Keys of the dictionary can be strings or an iterable of Substitutions
+ that will be expanded to a string.
+ Values in the dictionary can be strings, integers, floats, or tuples
+ of Substitutions that will be expanded to a string.
+ Additionally, values in the dictionary can be lists of the
+ aforementioned types, or another dictionary with the same properties.
+ A yaml file with the resulting parameters from the dictionary will be
+ written to a temporary file, the path to which will be passed to the
+ node.
+ Multiple dictionaries/files can be passed: each file path will be
+ passed in in order to the node (where the last definition of a
+ parameter takes effect).
+
+ :param: package the package in which the node executable can be found
+ :param: node_executable the name of the executable to find
+ :param: node_name the name of the node
+ :param: node_namespace the ros namespace for this Node
+ :param: parameters list of names of yaml files with parameter rules,
+ or dictionaries of parameters.
+ :param: use_remappings whether to apply the remappings at runtime.
+ :param: remappings ordered list of 'to' and 'from' string pairs to be
+ passed to the node as ROS remapping rules
+ :param: arguments list of extra arguments for the node
+ """
+ cmd = [ExecutableInPackage(package=package, executable=node_executable)]
+ cmd += [] if arguments is None else arguments
+ # Reserve space for ros specific arguments.
+ # The substitutions will get expanded when the action is executed.
+ cmd += ['--ros-args'] # Prepend ros specific arguments with --ros-args flag
+ if node_name is not None:
+ cmd += ['-r', LocalSubstitution(
+ "ros_specific_arguments['name']", description='node name')]
+ if parameters is not None:
+ ensure_argument_type(parameters, (list), 'parameters', 'Node')
+ # All elements in the list are paths to files with parameters (or substitutions that
+ # evaluate to paths), or dictionaries of parameters (fields can be substitutions).
+ i = 0
+ for param in parameters:
+ cmd += ['--params-file', LocalSubstitution(
+ "ros_specific_arguments['params'][{}]".format(i),
+ description='parameter {}'.format(i))]
+ i += 1
+ normalized_params = normalize_parameters(parameters)
+ if remappings is not None:
+ i = 0
+ for remapping in normalize_remap_rules(remappings):
+ k, v = remapping
+ # TODO(orduno) For some reason the remaps space should be reserved.
+ # If not reserved and cmd is extended (with remap args)
+ # on `execute`, the remaps are not applied, even when the
+ # command passed does include remap args
+ # e.g. --ros-args -r /tf:=tf
+ # For that reason we reserve the space here and together
+ # with the remap flag ('-r')
+ cmd += [LocalSubstitution("remap_flag"), LocalSubstitution(
+ "ros_specific_arguments['remaps'][{}]".format(i),
+ description='remapping {}'.format(i))]
+ i += 1
+ super().__init__(cmd=cmd, **kwargs)
+ self.__package = package
+ self.__node_executable = node_executable
+ self.__node_name = node_name
+ self.__node_namespace = node_namespace
+ self.__parameters = [] if parameters is None else normalized_params
+ self.__use_remappings = use_remappings
+ self.__remappings = [] if remappings is None else remappings
+ self.__arguments = arguments
+
+ self.__expanded_node_name = ''
+ self.__expanded_node_namespace = ''
+ self.__final_node_name = None # type: Optional[Text]
+ self.__expanded_parameter_files = None # type: Optional[List[Text]]
+ self.__expanded_remappings = None # type: Optional[List[Tuple[Text, Text]]]
+
+ self.__substitutions_performed = False
+
+ self.__logger = launch.logging.get_logger(__name__)
+
+ @staticmethod
+ def parse_nested_parameters(params, parser):
+ """Normalize parameters as expected by Node constructor argument."""
+ def get_nested_dictionary_from_nested_key_value_pairs(params):
+ """Convert nested params in a nested dictionary."""
+ param_dict = {}
+ for param in params:
+ name = tuple(parser.parse_substitution(param.get_attr('name')))
+ value = param.get_attr('value', data_type=None, optional=True)
+ nested_params = param.get_attr('param', data_type=List[Entity], optional=True)
+ if value is not None and nested_params:
+ raise RuntimeError('param and value attributes are mutually exclusive')
+ elif value is not None:
+ def normalize_scalar_value(value):
+ if isinstance(value, str):
+ value = parser.parse_substitution(value)
+ if len(value) == 1 and isinstance(value[0], TextSubstitution):
+ value = value[0].text # python `str` are not converted like yaml
+ return value
+ if isinstance(value, list):
+ value = [normalize_scalar_value(x) for x in value]
+ else:
+ value = normalize_scalar_value(value)
+ param_dict[name] = value
+ elif nested_params:
+ param_dict.update({
+ name: get_nested_dictionary_from_nested_key_value_pairs(nested_params)
+ })
+ else:
+ raise RuntimeError('either a value attribute or nested params are needed')
+ return param_dict
+
+ normalized_params = []
+ params_without_from = []
+ for param in params:
+ from_attr = param.get_attr('from', optional=True)
+ name = param.get_attr('name', optional=True)
+ if from_attr is not None and name is not None:
+ raise RuntimeError('name and from attributes are mutually exclusive')
+ elif from_attr is not None:
+ # 'from' attribute ignores 'name' attribute,
+ # it's not accepted to be nested,
+ # and it can not have children.
+ normalized_params.append(parser.parse_substitution(from_attr))
+ continue
+ elif name is not None:
+ params_without_from.append(param)
+ continue
+ raise ValueError('param Entity should have name or from attribute')
+ normalized_params.append(
+ get_nested_dictionary_from_nested_key_value_pairs(params_without_from))
+ return normalized_params
+
+ @classmethod
+ def parse(cls, entity: Entity, parser: Parser):
+ """Parse node."""
+ # See parse method of `ExecuteProcess`
+ _, kwargs = super().parse(entity, parser, 'args')
+ kwargs['arguments'] = kwargs['args']
+ del kwargs['args']
+ kwargs['node_name'] = kwargs['name']
+ del kwargs['name']
+ kwargs['package'] = parser.parse_substitution(entity.get_attr('pkg'))
+ kwargs['node_executable'] = parser.parse_substitution(entity.get_attr('exec'))
+ ns = entity.get_attr('namespace', optional=True)
+ if ns is not None:
+ kwargs['node_namespace'] = parser.parse_substitution(ns)
+ remappings = entity.get_attr('remap', optional=True)
+ if remappings is not None:
+ kwargs['remappings'] = [
+ (
+ parser.parse_substitution(remap.get_attr('from')),
+ parser.parse_substitution(remap.get_attr('to'))
+ ) for remap in remappings
+ ]
+ parameters = entity.get_attr('param', data_type=List[Entity], optional=True)
+ if parameters is not None:
+ kwargs['parameters'] = cls.parse_nested_parameters(parameters, parser)
+ return cls, kwargs
+
+ @property
+ def node_name(self):
+ """Getter for node_name."""
+ if self.__final_node_name is None:
+ raise RuntimeError("cannot access 'node_name' before executing action")
+ return self.__final_node_name
+
+ def _create_params_file_from_dict(self, params):
+ with NamedTemporaryFile(mode='w', prefix='launch_params_', delete=False) as h:
+ param_file_path = h.name
+ # TODO(dhood): clean up generated parameter files.
+ param_dict = {'/**': {'ros__parameters': params}}
+ yaml.dump(param_dict, h, default_flow_style=False)
+ return param_file_path
+
+ def _perform_substitutions(self, context: LaunchContext) -> None:
+ try:
+ if self.__substitutions_performed:
+ # This function may have already been called by a subclass' `execute`, for example.
+ return
+ self.__substitutions_performed = True
+ if self.__node_name is not None:
+ self.__expanded_node_name = perform_substitutions(
+ context, normalize_to_list_of_substitutions(self.__node_name))
+ validate_node_name(self.__expanded_node_name)
+ self.__expanded_node_name.lstrip('/')
+ self.__expanded_node_namespace = perform_substitutions(
+ context, normalize_to_list_of_substitutions(self.__node_namespace))
+ if not self.__expanded_node_namespace.startswith('/'):
+ base_ns = context.launch_configurations.get('ros_namespace', '')
+ self.__expanded_node_namespace = (
+ base_ns + '/' + self.__expanded_node_namespace
+ ).rstrip('/')
+ if (
+ self.__expanded_node_namespace != '' and not
+ self.__expanded_node_namespace.startswith('/')
+ ):
+ self.__expanded_node_namespace = '/' + self.__expanded_node_namespace
+ if self.__expanded_node_namespace != '':
+ cmd_extension = ['-r', LocalSubstitution("ros_specific_arguments['ns']")]
+ self.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_extension])
+ validate_namespace(self.__expanded_node_namespace)
+ except Exception:
+ self.__logger.error(
+ "Error while expanding or validating node name or namespace for '{}':"
+ .format('package={}, node_executable={}, name={}, namespace={}'.format(
+ self.__package,
+ self.__node_executable,
+ self.__node_name,
+ self.__node_namespace,
+ ))
+ )
+ raise
+ self.__final_node_name = ''
+ if self.__expanded_node_namespace not in ['', '/']:
+ self.__final_node_name += self.__expanded_node_namespace
+ self.__final_node_name += '/' + self.__expanded_node_name
+ # expand parameters too
+ if self.__parameters is not None:
+ self.__expanded_parameter_files = []
+ evaluated_parameters = evaluate_parameters(context, self.__parameters)
+ for params in evaluated_parameters:
+ if isinstance(params, dict):
+ param_file_path = self._create_params_file_from_dict(params)
+ elif isinstance(params, pathlib.Path):
+ param_file_path = str(params)
+ else:
+ raise RuntimeError('invalid normalized parameters {}'.format(repr(params)))
+ if not os.path.isfile(param_file_path):
+ self.__logger.warning(
+ 'Parameter file path is not a file: {}'.format(param_file_path),
+ )
+ # Don't skip adding the file to the parameter list since space has been
+ # reserved for it in the ros_specific_arguments.
+ self.__expanded_parameter_files.append(param_file_path)
+ # expand remappings too
+ if self.__remappings is not None:
+ self.__expanded_remappings = []
+ for k, v in self.__remappings:
+ key = perform_substitutions(context, normalize_to_list_of_substitutions(k))
+ value = perform_substitutions(context, normalize_to_list_of_substitutions(v))
+ self.__expanded_remappings.append((key, value))
+
+ def execute(self, context: LaunchContext) -> Optional[List[Action]]:
+ """
+ Execute the action.
+
+ Delegated to :meth:`launch.actions.ExecuteProcess.execute`.
+ """
+ self._perform_substitutions(context)
+ # Prepare the ros_specific_arguments list and add it to the context so that the
+ # LocalSubstitution placeholders added to the the cmd can be expanded using the contents.
+ ros_specific_arguments: Dict[str, Union[str, List[str]]] = {}
+ if self.__node_name is not None:
+ ros_specific_arguments['name'] = '__node:={}'.format(self.__expanded_node_name)
+ if self.__expanded_node_namespace != '':
+ ros_specific_arguments['ns'] = '__ns:={}'.format(self.__expanded_node_namespace)
+ if self.__expanded_parameter_files is not None:
+ ros_specific_arguments['params'] = self.__expanded_parameter_files
+ if self.__expanded_remappings is not None:
+ ros_specific_arguments['remaps'] = []
+ for remapping_from, remapping_to in self.__expanded_remappings:
+ remap_arguments = cast(List[str], ros_specific_arguments['remaps'])
+ if self.__use_remappings is None or self.__use_remappings.evaluate(context):
+ remap_arguments.append('{}:={}'.format(remapping_from, remapping_to))
+ else:
+ remap_arguments.append('')
+ if self.__use_remappings is None or self.__use_remappings.evaluate(context):
+ context.extend_locals({'remap_flag': '-r'})
+ else:
+ context.extend_locals({'remap_flag': ''})
+ context.extend_locals({'ros_specific_arguments': ros_specific_arguments})
+ return super().execute(context)
+
+ @property
+ def expanded_node_namespace(self):
+ """Getter for expanded_node_namespace."""
+ return self.__expanded_node_namespace
diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py
index 5837b64923..2e521b4ca7 100644
--- a/nav2_common/nav2_common/launch/rewritten_yaml.py
+++ b/nav2_common/nav2_common/launch/rewritten_yaml.py
@@ -15,6 +15,7 @@
from typing import Dict
from typing import List
from typing import Text
+from typing import Optional
import yaml
import tempfile
import launch
@@ -37,16 +38,21 @@ class RewrittenYaml(launch.Substitution):
def __init__(self,
source_file: launch.SomeSubstitutionsType,
- rewrites: Dict,
+ param_rewrites: Dict,
+ key_rewrites: Optional[Dict] = None,
convert_types = False) -> 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.__rewrites = {}
+ self.__param_rewrites = {}
+ self.__key_rewrites = {}
self.__convert_types = convert_types
- for key in rewrites:
- self.__rewrites[key] = normalize_to_list_of_substitutions(rewrites[key])
+ for key in param_rewrites:
+ self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key])
+ if key_rewrites is not None:
+ for key in key_rewrites:
+ self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key])
@property
def name(self) -> List[launch.Substitution]:
@@ -60,29 +66,42 @@ def describe(self) -> Text:
def perform(self, context: launch.LaunchContext) -> Text:
yaml_filename = launch.utilities.perform_substitutions(context, self.name)
rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False)
- resolved_rewrites = self.resolve_rewrites(context)
+ param_rewrites, keys_rewrites = self.resolve_rewrites(context)
data = yaml.safe_load(open(yaml_filename, 'r'))
- self.substitute_values(data, resolved_rewrites)
+ self.substitute_params(data, param_rewrites)
+ self.substitute_keys(data, keys_rewrites)
yaml.dump(data, rewritten_yaml)
rewritten_yaml.close()
return rewritten_yaml.name
def resolve_rewrites(self, context):
- resolved = {}
- for key in self.__rewrites:
- resolved[key] = launch.utilities.perform_substitutions(context, self.__rewrites[key])
- return resolved
-
- def substitute_values(self, yaml, rewrites):
- for key in self.getYamlKeys(yaml):
- if key.key() in rewrites:
- raw_value = rewrites[key.key()]
+ resolved_params = {}
+ for key in self.__param_rewrites:
+ resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key])
+ resolved_keys = {}
+ for key in self.__key_rewrites:
+ resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key])
+ return resolved_params, resolved_keys
+
+ def substitute_params(self, yaml, param_rewrites):
+ for key in self.getYamlLeafKeys(yaml):
+ if key.key() in param_rewrites:
+ raw_value = param_rewrites[key.key()]
key.setValue(self.convert(raw_value))
- def getYamlKeys(self, yamlData):
+ def substitute_keys(self, yaml, key_rewrites):
+ if len(key_rewrites) != 0:
+ for key, val in yaml.items():
+ if isinstance(val, dict) and key in key_rewrites:
+ new_key = key_rewrites[key]
+ yaml[new_key] = yaml[key]
+ del yaml[key]
+ self.substitute_keys(val, key_rewrites)
+
+ def getYamlLeafKeys(self, yamlData):
try:
for key in yamlData.keys():
- for k in self.getYamlKeys(yamlData[key]):
+ for k in self.getYamlLeafKeys(yamlData[key]):
yield k
yield DictItemReference(yamlData, key)
except AttributeError:
diff --git a/nav2_common/package.xml b/nav2_common/package.xml
index 9bee99b82b..eddcb9e414 100644
--- a/nav2_common/package.xml
+++ b/nav2_common/package.xml
@@ -7,6 +7,12 @@
Carl Delsey
Apache-2.0
+ launch
+ launch_ros
+ osrf_pycommon
+ rclpy
+ python3-yaml
+
ament_cmake_core
ament_cmake_python
diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp
index 5229f0f81b..06ea009b70 100644
--- a/nav2_controller/src/nav2_controller.cpp
+++ b/nav2_controller/src/nav2_controller.cpp
@@ -38,7 +38,7 @@ ControllerServer::ControllerServer()
// The costmap node is used in the implementation of the local planner
costmap_ros_ = std::make_shared(
- "local_costmap", nav2_util::add_namespaces(std::string{get_namespace()}, "local_costmap"));
+ "local_costmap", std::string{get_namespace()}, "local_costmap");
// Launch a thread to run the costmap node
costmap_thread_ = std::make_unique(
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
index 5e34610c94..8a8c812df4 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
@@ -82,9 +82,11 @@ class Costmap2DROS : public nav2_util::LifecycleNode
/**
* @brief Constructor for the wrapper
* @param name Name of the costmap ROS node
- * @param absolute_namespace Namespace of the costmap ROS node starting with "/"
+ * @param parent_namespace Absolute namespace of the node hosting the costmap node
+ * @param local_namespace Namespace to append to the parent namespace
*/
- explicit Costmap2DROS(const std::string & name, const std::string & absolute_namespace);
+ explicit Costmap2DROS(const std::string & name,
+ const std::string & parent_namespace, const std::string & local_namespace);
~Costmap2DROS();
@@ -262,6 +264,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode
LayeredCostmap * layered_costmap_{nullptr};
std::string name_;
+ std::string parent_namespace_;
void mapUpdateLoop(double frequency);
bool map_update_thread_shutdown_{false};
bool stop_updates_{false};
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index 9b2647b49c..9b92481ae7 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -44,6 +44,7 @@
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_util/execution_timer.hpp"
+#include "nav2_util/node_utils.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/create_timer_ros.h"
#include "nav2_util/robot_utils.hpp"
@@ -53,15 +54,18 @@ using namespace std::chrono_literals;
namespace nav2_costmap_2d
{
Costmap2DROS::Costmap2DROS(const std::string & name)
-: Costmap2DROS(name, name) {}
+: Costmap2DROS(name, "/", name) {}
-Costmap2DROS::Costmap2DROS(const std::string & name, const std::string & absolute_namespace)
+Costmap2DROS::Costmap2DROS(const std::string & name,
+ const std::string & parent_namespace, const std::string & local_namespace)
: nav2_util::LifecycleNode(name, "", true,
// NodeOption arguments take precedence over the ones provided on the command line
// use this to make sure the node is placed on the provided namespace
- rclcpp::NodeOptions().arguments({"--ros-args", "-r", std::string(
- "__ns:=") + absolute_namespace})),
- name_(name)
+ // TODO(orduno) Pass a sub-node instead of creating a new node for better handling
+ // of the namespaces
+ rclcpp::NodeOptions().arguments({"--ros-args", "-r", std::string("__ns:=") +
+ nav2_util::add_namespaces(parent_namespace, local_namespace)})),
+ name_(name), parent_namespace_(parent_namespace)
{
RCLCPP_INFO(get_logger(), "Creating Costmap");
auto options = rclcpp::NodeOptions().arguments(
@@ -79,7 +83,8 @@ Costmap2DROS::Costmap2DROS(const std::string & name, const std::string & absolut
declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map")));
declare_parameter("height", rclcpp::ParameterValue(10));
declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
- declare_parameter("map_topic", rclcpp::ParameterValue(std::string("/map")));
+ declare_parameter("map_topic", rclcpp::ParameterValue(
+ (parent_namespace_=="/" ? "" : parent_namespace_ ) + std::string("/map")));
declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("")));
declare_parameter("origin_x", rclcpp::ParameterValue(0.0));
declare_parameter("origin_y", rclcpp::ParameterValue(0.0));
@@ -137,6 +142,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
// TODO(mjeronimo): instead of get(), use a shared ptr
plugin->initialize(layered_costmap_, plugin_names_[i], tf_buffer_.get(),
shared_from_this(), client_node_, rclcpp_node_);
+
+ RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str());
}
// Create the publishers and subscribers
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index 24044d1136..1f355d3889 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -45,8 +45,7 @@ PlannerServer::PlannerServer()
// Setup the global costmap
costmap_ros_ = std::make_shared(
- "global_costmap", nav2_util::add_namespaces(std::string{get_namespace()},
- "global_costmap"));
+ "global_costmap", std::string{get_namespace()}, "global_costmap");
// Launch a thread to run the costmap node
costmap_thread_ = std::make_unique(