def test_evaluate_condition_expression():
"""Test common use cases for evaluate_condition_expression()."""
class MockLaunchContext:
def perform_substitution(self, substitution):
return substitution.perform(self)
lc = MockLaunchContext()
test_cases = [
('true', True),
('True', True),
('TRUE', True),
('1', True),
('false', False),
('False', False),
('FALSE', False),
('0', False),
]
for string, expected in test_cases:
assert evaluate_condition_expression(
lc, [TextSubstitution(text=string)]) is expected
with pytest.raises(InvalidConditionExpressionError):
evaluate_condition_expression(lc, [TextSubstitution(text='')])
with pytest.raises(InvalidConditionExpressionError):
evaluate_condition_expression(lc, [TextSubstitution(text='typo')])
def __init__(self, *, world, gui, mode):
self.__gui = gui if isinstance(
gui, Substitution) else TextSubstitution(text=str(gui))
self.__mode = mode if isinstance(
mode, Substitution) else TextSubstitution(text=mode)
self.__world = world if isinstance(
world, Substitution) else TextSubstitution(text=world)
def test_dictionary_with_dissimilar_array():
orig = [{'foo': 1, 'fiz': [True, 2.0, 3]}]
norm = normalize_parameters(orig)
expected = ({'foo': 1, 'fiz': ('True', '2.0', '3')}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{'foo': 1, 'fiz': [True, 1, TextSubstitution(text='foo')]}]
norm = normalize_parameters(orig)
expected = ({'foo': 1, 'fiz': ('True', '1', 'foo')}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{'foo': 1, 'fiz': [TextSubstitution(text='foo'), True, 1]}]
norm = normalize_parameters(orig)
expected = ({'foo': 1, 'fiz': ('foo', 'True', '1')}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{'foo': 1, 'fiz': [True, 1, [TextSubstitution(text='foo')]]}]
norm = normalize_parameters(orig)
expected = ({'foo': 1, 'fiz': ('True', '1', 'foo')}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{'foo': 1, 'fiz': [[TextSubstitution(text='foo')], True, 1]}]
norm = normalize_parameters(orig)
expected = ({'foo': 1, 'fiz': ('foo', 'True', '1')}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
def test_if_condition():
"""Test UnlessCondition class."""
class MockLaunchContext:
def perform_substitution(self, substitution):
return substitution.perform(self)
lc = MockLaunchContext()
test_cases = [
('true', True),
('True', True),
('TRUE', True),
('1', True),
('false', False),
('False', False),
('FALSE', False),
('0', False),
]
for string, expected in test_cases:
assert UnlessCondition([TextSubstitution(text=string)]).evaluate(lc) is not expected
with pytest.raises(InvalidConditionExpressionError):
UnlessCondition([TextSubstitution(text='')]).evaluate(lc)
with pytest.raises(InvalidConditionExpressionError):
UnlessCondition([TextSubstitution(text='typo')]).evaluate(lc)
def _parse_cmdline(cls, cmd: Text,
parser: Parser) -> List[SomeSubstitutionsType]:
"""
Parse text apt for command line execution.
:param: cmd a space (' ') delimited command line arguments list.
All found `TextSubstitution` items are split and added to the
list again as a `TextSubstitution`.
:return: a list of command line arguments.
"""
result_args = []
arg: List[SomeSubstitutionsType] = []
def _append_arg():
nonlocal arg
result_args.append(arg)
arg = []
for sub in parser.parse_substitution(cmd):
if isinstance(sub, TextSubstitution):
tokens = shlex.split(sub.text)
if not tokens:
# Sting with just spaces.
# Appending args allow splitting two substitutions
# separated by a space.
# e.g.: `$(subst1 asd) $(subst2 bsd)` will be two separate arguments.
_append_arg()
continue
if sub.text[0].isspace():
# Needed for splitting from the previous argument
# e.g.: `$(find-exec bsd) asd`
# It splits `asd` from the path of `bsd` executable.
if len(arg) != 0:
_append_arg()
arg.append(TextSubstitution(text=tokens[0]))
if len(tokens) > 1:
# Needed to split the first argument when more than one token.
# e.g. `$(find-pkg-prefix csd)/asd bsd`
# will split `$(find-pkg-prefix csd)/asd` from `bsd`.
_append_arg()
arg.append(TextSubstitution(text=tokens[-1]))
if len(tokens) > 2:
# If there are more than two tokens, just add all the middle tokens to
# `result_args`.
# e.g. `$(find-pkg-prefix csd)/asd bsd dsd xsd`
# 'bsd' 'dsd' will be added.
result_args.extend([TextSubstitution(text=x)]
for x in tokens[1:-1])
if sub.text[-1].isspace():
# Allows splitting from next argument.
# e.g. `exec $(find-some-file)`
# Will split `exec` argument from the result of `find-some-file` substitution.
_append_arg()
else:
arg.append(sub)
if arg:
result_args.append(arg)
return result_args
def test_is_normalized_substitution():
assert is_normalized_substitution([TextSubstitution(text='asd')])
assert is_normalized_substitution(
[TextSubstitution(text='asd'),
TextSubstitution(text='bsd')])
assert not is_normalized_substitution(TextSubstitution(text='asd'))
assert not is_normalized_substitution(
[TextSubstitution(text='asd'), 'bsd'])
assert not is_normalized_substitution(['bsd'])
assert not is_normalized_substitution('bsd')
def test_dictionary_with_substitution():
orig = [{TextSubstitution(text='bar'): TextSubstitution(text='baz')}]
norm = normalize_parameters(orig)
expected = ({'bar': 'baz'}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
# substitutions get yamlfied
orig = [{TextSubstitution(text='false'): TextSubstitution(text='off')}]
norm = normalize_parameters(orig)
expected = ({'false': False}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
def generate_launch_description():
sim_arg = DeclareLaunchArgument('sim', default_value='False')
fake_walk_arg = DeclareLaunchArgument('fake_walk', default_value='False')
sim_ns_arg = DeclareLaunchArgument(
'sim_ns', default_value=TextSubstitution(text='/'))
robot_type_arg = DeclareLaunchArgument(
'robot_type', default_value=TextSubstitution(text='wolfgang'))
return LaunchDescription([
sim_arg, fake_walk_arg, sim_ns_arg, robot_type_arg,
OpaqueFunction(function=launch_setup)
])
def test_append_environment_variable_execute():
"""Test the execute() of the AppendEnvironmentVariable class."""
lc1 = LaunchContext()
# Sets environment variable if it does not exist
if 'NONEXISTENT_KEY' in os.environ:
del os.environ['NONEXISTENT_KEY']
assert os.environ.get('NONEXISTENT_KEY') is None
AppendEnvironmentVariable('NONEXISTENT_KEY', 'value').visit(lc1)
assert os.environ.get('NONEXISTENT_KEY') == 'value'
# Same result if prepending is enabled
del os.environ['NONEXISTENT_KEY']
AppendEnvironmentVariable('NONEXISTENT_KEY', 'value', prepend=True).visit(lc1)
assert os.environ.get('NONEXISTENT_KEY') == 'value'
# Appends to environment variable if it does exist
AppendEnvironmentVariable('NONEXISTENT_KEY', 'another value').visit(lc1)
assert os.environ.get('NONEXISTENT_KEY') == 'value' + os.pathsep + 'another value'
# Prepends to environment variable if it does exist and option is enabled
AppendEnvironmentVariable('NONEXISTENT_KEY', 'some value', prepend=True).visit(lc1)
assert os.environ.get('NONEXISTENT_KEY') == \
'some value' + os.pathsep + 'value' + os.pathsep + 'another value'
# Can use an optional separator
AppendEnvironmentVariable('NONEXISTENT_KEY', 'other value', separator='|').visit(lc1)
assert os.environ.get('NONEXISTENT_KEY') == \
'some value' + os.pathsep + 'value' + os.pathsep + 'another value' + '|' + 'other value'
# Appends/prepends with substitutions
assert os.environ.get('ANOTHER_NONEXISTENT_KEY') is None
AppendEnvironmentVariable(
'ANOTHER_NONEXISTENT_KEY',
EnvironmentVariable('NONEXISTENT_KEY'),
prepend=TextSubstitution(text='false')).visit(lc1)
assert os.environ.get('ANOTHER_NONEXISTENT_KEY') == \
'some value' + os.pathsep + 'value' + os.pathsep + 'another value' + '|' + 'other value'
os.environ['ANOTHER_NONEXISTENT_KEY'] = 'abc'
os.environ['SOME_SEPARATOR'] = '//'
AppendEnvironmentVariable(
'ANOTHER_NONEXISTENT_KEY',
TextSubstitution(text='def'),
separator=EnvironmentVariable('SOME_SEPARATOR'),
prepend=TextSubstitution(text='yes')).visit(lc1)
assert os.environ.get('ANOTHER_NONEXISTENT_KEY') == 'def' + '//' + 'abc'
# Cleanup environment variables
del os.environ['NONEXISTENT_KEY']
del os.environ['ANOTHER_NONEXISTENT_KEY']
del os.environ['SOME_SEPARATOR']
def test_dictionary_with_substitution_list_value():
orig = [{
'foo': [TextSubstitution(text='fiz'),
TextSubstitution(text='buz')]
}]
norm = normalize_parameters(orig)
expected = ({'foo': 'fizbuz'}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{
'foo': [[TextSubstitution(text='fiz')], [TextSubstitution(text='buz')]]
}]
norm = normalize_parameters(orig)
expected = ({'foo': ('fiz', 'buz')}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
def test_is_substitution():
assert is_substitution(TextSubstitution(text='asd'))
assert is_substitution([TextSubstitution(text='asd'), 'bsd'])
assert is_substitution([
'asd',
TextSubstitution(text='bsd'),
])
assert is_substitution([
TextSubstitution(text='asd'),
TextSubstitution(text='bsd'),
])
assert not is_substitution([])
assert not is_substitution('asd')
assert not is_substitution(['asd', 'bsd'])
assert not is_substitution(['asd', 'bsd'])
assert not is_substitution(1)
def test_launch_configuration_not_equals():
"""Test LaunchConfigurationNotEquals class."""
class MockLaunchContext:
def perform_substitution(self, substitution):
return substitution.perform(self)
@property
def launch_configurations(self):
return {
'foo': 'foo_value',
'bar': 'bar_value',
'empty': '',
}
lc = MockLaunchContext()
test_cases = [
('foo', 'foo_value', False),
('bar', 'bar_value', False),
('bar', 'foo_value', True),
('bar', None, True),
('empty', '', False),
('empty', 'foo_value', True),
('empty', None, True),
('baz', None, False),
('baz', 'foo_value', True),
]
for name, value, expected in test_cases:
assert LaunchConfigurationNotEquals(
name, [TextSubstitution(text=value)]
if value is not None else None).evaluate(lc) is expected
def test_action_substitutions(self) -> None:
self.assertIsNone(os.environ.get('LD_PRELOAD'))
tmpdir = tempfile.mkdtemp(
prefix='TestTraceAction__test_action_substitutions')
self.assertIsNone(os.environ.get('TestTraceAction__event_ust', None))
os.environ['TestTraceAction__event_ust'] = 'ros2:*'
self.assertIsNone(
os.environ.get('TestTraceAction__context_field', None))
os.environ['TestTraceAction__context_field'] = 'vpid'
session_name_arg = DeclareLaunchArgument(
'session-name',
default_value='my-session-name',
description='the session name',
)
action = Trace(
session_name=LaunchConfiguration(session_name_arg.name),
base_path=TextSubstitution(text=tmpdir),
events_kernel=[],
events_ust=[
EnvironmentVariable(name='TestTraceAction__event_ust'),
TextSubstitution(text='*'),
],
context_fields={
'kernel': [],
'userspace': [
EnvironmentVariable(name='TestTraceAction__context_field'),
TextSubstitution(text='vtid'),
],
},
)
self._assert_launch_no_errors([session_name_arg, action])
self._check_trace_action(action, tmpdir)
self.assertDictEqual(
action.context_fields,
{
'kernel': [],
'userspace': ['vpid', 'vtid'],
},
)
shutil.rmtree(tmpdir)
del os.environ['TestTraceAction__event_ust']
del os.environ['TestTraceAction__context_field']
del os.environ['LD_PRELOAD']
def test_launch_context_perform_substitution():
"""Test performing substitutions with LaunchContext class."""
lc = LaunchContext()
from launch.substitutions import TextSubstitution
sub = TextSubstitution(text='foo')
assert lc.perform_substitution(sub) == 'foo'
def test_mixed_substitutions():
lc = LaunchContext()
rule = (('foo', 'bar'), ['bar', TextSubstitution(text='baz')])
output_rule = normalize_remap_rule(rule)
assert isinstance(output_rule, tuple)
assert len(output_rule) == 2
assert 'foobar' == perform_substitutions(lc, output_rule[0])
assert 'barbaz' == perform_substitutions(lc, output_rule[1])
def generate_launch_description():
share_dir = get_package_share_directory('serial_driver')
node_name = 'serial_bridge_node'
params_declare = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(share_dir, 'params', 'example.params.yaml'),
description='File path to the ROS2 parameters file to use')
bridge_node = LifecycleNode(
package='serial_driver',
executable='serial_bridge',
name=node_name,
namespace=TextSubstitution(text=''),
parameters=[LaunchConfiguration('params_file')],
output='screen',
)
configure_event_handler = RegisterEventHandler(
event_handler=OnProcessStart(
target_action=bridge_node,
on_start=[
EmitEvent(event=ChangeState(
lifecycle_node_matcher=matches_action(bridge_node),
transition_id=Transition.TRANSITION_CONFIGURE,
), ),
],
))
activate_event_handler = RegisterEventHandler(
event_handler=OnStateTransition(
target_lifecycle_node=bridge_node,
start_state='configuring',
goal_state='inactive',
entities=[
EmitEvent(event=ChangeState(
lifecycle_node_matcher=matches_action(bridge_node),
transition_id=Transition.TRANSITION_ACTIVATE,
), ),
],
))
shutdown_event_handler = RegisterEventHandler(event_handler=OnShutdown(
on_shutdown=[
EmitEvent(event=ChangeState(
lifecycle_node_matcher=matches_node_name(node_name),
transition_id=Transition.TRANSITION_ACTIVE_SHUTDOWN,
))
]))
return LaunchDescription([
params_declare,
bridge_node,
configure_event_handler,
activate_event_handler,
shutdown_event_handler,
])
def test_multiple_rules():
lc = LaunchContext()
rules = [('ping', 'pong'),
(('baz', 'foo'), ['bar', TextSubstitution(text='baz')])]
output_rules = list(normalize_remap_rules(rules))
assert len(rules) == len(output_rules)
assert 'ping' == perform_substitutions(lc, output_rules[0][0])
assert 'pong' == perform_substitutions(lc, output_rules[0][1])
assert 'bazfoo' == perform_substitutions(lc, output_rules[1][0])
assert 'barbaz' == perform_substitutions(lc, output_rules[1][1])
def test_dictionary_with_mixed_substitutions_and_strings():
orig = [{'foo': [TextSubstitution(text='fiz'), 'bar']}]
norm = normalize_parameters(orig)
expected = ({'foo': 'fizbar'}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{'foo': [[TextSubstitution(text='fiz')], 'bar']}]
norm = normalize_parameters(orig)
expected = ({'foo': ('fiz', 'bar')}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{'foo': ['bar', TextSubstitution(text='fiz')]}]
norm = normalize_parameters(orig)
expected = ({'foo': 'barfiz'}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{'foo': ['bar', [TextSubstitution(text='fiz')]]}]
norm = normalize_parameters(orig)
expected = ({'foo': ('bar', 'fiz')}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
def get_set_parameter_test_parameters():
return [
pytest.param(
[('my_param', '2')], # to set
{'my_param': '2'}, # expected
id='One param'),
pytest.param([('my_param', '2'), ('another_param', '2'),
('my_param', '3')], {
'my_param': '3',
'another_param': '2'
},
id='Two params, overriding one'),
pytest.param([(TextSubstitution(text='my_param'),
TextSubstitution(text='my_value'))],
{'my_param': 'my_value'},
id='Substitution types'),
pytest.param([((TextSubstitution(text='my_param'), ),
(TextSubstitution(text='my_value'), ))],
{'my_param': 'my_value'},
id='Tuple of substitution types'),
pytest.param([([TextSubstitution(text='my_param')
], [TextSubstitution(text='my_value')])],
{'my_param': 'my_value'},
id='List of substitution types'),
pytest.param([('my_param', ParameterValue('my_value'))],
{'my_param': 'my_value'},
id='ParameterValue type'),
]
def initialize_robots(context, *args, **kwargs):
"""initialize robots"""
base_frame = LaunchConfiguration('base_frame').perform(context)
robots_file = LaunchConfiguration('robots_file').perform(context)
spawner_dir = get_package_share_directory("robot_spawner_pkg")
with open(robots_file, 'r') as stream:
robots = yaml.safe_load(stream)
nav_bringup_cmds = []
for robot in robots:
nav_bringup_cmds.append(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(spawner_dir, "single_real_robot.launch.py")),
launch_arguments={
'x_pose': TextSubstitution(text=str(robot['x_pose'])),
'y_pose': TextSubstitution(text=str(robot['y_pose'])),
'z_pose': TextSubstitution(text=str(robot['z_pose'])),
'robot_name': TextSubstitution(text=str(robot['name'])),
'yaw_pose': TextSubstitution(text=str(robot['yaw_pose'])),
'base_frame': TextSubstitution(text=base_frame),
# 'turtlebot_type': TextSubstitution(text='burger')
}.items()))
return nav_bringup_cmds
def test_parameter_value_description():
lc = MockContext()
param = ParameterValue(value='asd')
assert param.value == 'asd'
assert param.value_type is None
assert param.evaluate(lc) == 'asd'
# After the first `evaluate` call, the following `.value` and `.evaluate()`
# calls are calculated differently. Test them too.
assert param.value == 'asd'
assert param.evaluate(lc) == 'asd'
param = ParameterValue(value='asd', value_type=str)
assert param.value == 'asd'
assert param.value_type is str
assert param.evaluate(lc) == 'asd'
assert param.value == 'asd'
assert param.evaluate(lc) == 'asd'
param = ParameterValue(value=TextSubstitution(text='1'))
assert isinstance(param.value, list)
assert len(param.value) == 1
assert isinstance(param.value[0], TextSubstitution)
assert param.evaluate(lc) == 1
assert param.value == 1
assert param.evaluate(lc) == 1
param = ParameterValue(
value=[
'[',
TextSubstitution(text='1, '),
TextSubstitution(text='2, '),
TextSubstitution(text='3, '),
']',
],
value_type=List[int],
)
assert isinstance(param.value, list)
assert param.evaluate(lc) == [1, 2, 3]
assert param.value == [1, 2, 3]
assert param.evaluate(lc) == [1, 2, 3]
param = ParameterValue(value=TextSubstitution(text='[1, 2, 3]'), )
assert isinstance(param.value, list)
assert len(param.value) == 1
assert isinstance(param.value[0], TextSubstitution)
assert param.evaluate(lc) == [1, 2, 3]
assert param.value == [1, 2, 3]
assert param.evaluate(lc) == [1, 2, 3]
with pytest.raises(TypeError):
ParameterValue(value='1', value_type=int)
param = ParameterValue(value=TextSubstitution(text='[1, asd, 3]'))
with pytest.raises(ValueError):
param.evaluate(lc)
def initialize_robots(context, *args, **kwargs):
"""initialize robots"""
base_frame = LaunchConfiguration('base_frame').perform(context)
robot_name_list = []
robot_name_list_dir = os.path.join(
get_package_share_directory('system_status'), 'local_ips.yaml')
with open(robot_name_list_dir, 'r') as f:
data = yaml.safe_load(f)
robot_name_list = data[0]['local_ips']
nav_bringup_cmds = []
for robot_name in robot_name_list:
robot_name = 'robot' + robot_name
nav_bringup_cmds.append(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[ThisLaunchFileDir(), "/single_real_robot.launch.py"]),
launch_arguments={
'robot_name': TextSubstitution(text=robot_name),
'base_frame': TextSubstitution(text=base_frame),
# 'turtlebot_type': TextSubstitution(text='burger')
}.items()))
return nav_bringup_cmds
def test_unallowed_yaml_types_as_strings():
# All the tests from test_unallowed_yaml_types_in_substitutions
# but coerced to the proper type with ParameterValue
orig = [{'foo': 1, 'fiz': ParameterValue(TextSubstitution(text="{'asd': 3}"), value_type=str)}]
norm = normalize_parameters(orig)
expected = ({'foo': 1, 'fiz': "{'asd': 3}"},)
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{'foo': 1, 'fiz': ParameterValue(TextSubstitution(text='[1, 2.0, 3]'),
value_type=str)}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
expected = ({'foo': 1, 'fiz': '[1, 2.0, 3]'},)
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{'foo': 1, 'fiz': ParameterValue(TextSubstitution(text='[[2, 3], [2, 3], [2, 3]]'),
value_type=str)}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
expected = ({'foo': 1, 'fiz': '[[2, 3], [2, 3], [2, 3]]'},)
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{'foo': 1, 'fiz': ParameterValue(TextSubstitution(text='[]'), value_type=str)}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
expected = ({'foo': 1, 'fiz': '[]'},)
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{
'foo': 1,
'fiz': ParameterValue([
[TextSubstitution(text="['asd', 'bsd']")],
[TextSubstitution(text="['asd', 'csd']")]
], value_type=List[str])
}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
expected = ({'foo': 1, 'fiz': ["['asd', 'bsd']", "['asd', 'csd']"]},)
assert evaluate_parameters(LaunchContext(), norm) == expected
orig = [{'foo': 1,
'fiz': ParameterValue(TextSubstitution(text='Text That : Cannot Be Parsed As : Yaml'),
value_type=str)}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
expected = ({'foo': 1, 'fiz': 'Text That : Cannot Be Parsed As : Yaml'},)
assert evaluate_parameters(LaunchContext(), norm) == expected
def test_unallowed_yaml_types_in_substitutions():
with pytest.raises(TypeError) as exc:
orig = [{'foo': 1, 'fiz': TextSubstitution(text="{'asd': 3}")}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Allowed value types' in str(exc.value)
assert 'dict' in str(exc.value)
with pytest.raises(TypeError) as exc:
orig = [{'foo': 1, 'fiz': TextSubstitution(text='[1, 2.0, 3]')}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Expected a non-empty sequence' in str(exc.value)
with pytest.raises(TypeError) as exc:
orig = [{'foo': 1, 'fiz': TextSubstitution(text='[[2, 3], [2, 3], [2, 3]]')}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Expected a non-empty sequence' in str(exc.value)
with pytest.raises(TypeError) as exc:
orig = [{'foo': 1, 'fiz': TextSubstitution(text='[]')}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Expected a non-empty sequence' in str(exc.value)
with pytest.raises(TypeError) as exc:
orig = [{
'foo': 1,
'fiz': [
[TextSubstitution(text="['asd', 'bsd']")],
[TextSubstitution(text="['asd', 'csd']")]
]
}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Expected a non-empty sequence' in str(exc.value)
with pytest.raises(TypeError) as exc:
orig = [{'foo': 1, 'fiz': TextSubstitution(text='Text That : Cannot Be Parsed As : Yaml')}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Unable to parse' in str(exc.value)
def test_dictionary_with_dissimilar_array():
with pytest.raises(TypeError) as exc:
orig = [{'foo': 1, 'fiz': [True, 2.0, 3]}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Expected a non-empty' in str(exc.value)
with pytest.raises(TypeError) as exc:
orig = [{'foo': 1, 'fiz': [True, 1, TextSubstitution(text='foo')]}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Expected a non-empty' in str(exc.value)
with pytest.raises(TypeError) as exc:
orig = [{'foo': 1, 'fiz': [TextSubstitution(text='foo'), True, 1]}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Expected a non-empty' in str(exc.value)
with pytest.raises(TypeError) as exc:
orig = [{'foo': 1, 'fiz': [True, 1, [TextSubstitution(text='foo')]]}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Expected a non-empty' in str(exc.value)
with pytest.raises(TypeError) as exc:
orig = [{'foo': 1, 'fiz': [[TextSubstitution(text='foo')], True, 1]}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Expected a non-empty' in str(exc.value)
with pytest.raises(TypeError) as exc:
orig = [{
'foo': [
[TextSubstitution(text='True')],
[TextSubstitution(text='2.0')],
[TextSubstitution(text='3')],
]
}]
norm = normalize_parameters(orig)
evaluate_parameters(LaunchContext(), norm)
assert 'Expected a non-empty' in str(exc.value)
def get_set_parameter_test_parameters():
return [
pytest.param(
[('my_param', '2')], # to set
[('my_param', '2')], # expected
id='One param'),
pytest.param([(TextSubstitution(text='my_param'),
TextSubstitution(text='my_value'))],
[('my_param', 'my_value')],
id='Substitution types'),
pytest.param([((TextSubstitution(text='my_param'), ),
(TextSubstitution(text='my_value'), ))],
[('my_param', 'my_value')],
id='Tuple of substitution types'),
pytest.param([([TextSubstitution(text='my_param')
], [TextSubstitution(text='my_value')])],
[('my_param', 'my_value')],
id='List of substitution types'),
pytest.param([('my_param', ParameterValue('my_value'))],
[('my_param', 'my_value')],
id='ParameterValue type'),
]
def test_dictionary_with_substitution_list_name():
orig = [{(TextSubstitution(text='bar'), TextSubstitution(text='foo')): 1}]
norm = normalize_parameters(orig)
expected = ({'barfoo': 1}, )
assert evaluate_parameters(LaunchContext(), norm) == expected
def test_path_with_substitutions():
orig = [(TextSubstitution(text='/foo'), TextSubstitution(text='/bar'))]
norm = normalize_parameters(orig)
expected = (pathlib.Path('/foo/bar'), )
assert evaluate_parameters(LaunchContext(), norm) == expected
def generate_launch_description():
map_yaml_file = os.getenv('TEST_MAP')
world = os.getenv('TEST_WORLD')
urdf = os.getenv('TEST_URDF')
sdf = os.getenv('TEST_SDF')
bt_xml_file = os.path.join(
get_package_share_directory('nav2_bt_navigator'), 'behavior_trees',
os.getenv('BT_NAVIGATOR_XML'))
bringup_dir = get_package_share_directory('nav2_bringup')
robot1_params_file = os.path.join(
bringup_dir, # noqa: F841
'params/nav2_multirobot_params_1.yaml')
robot2_params_file = os.path.join(
bringup_dir, # noqa: F841
'params/nav2_multirobot_params_2.yaml')
# Names and poses of the robots
robots = [{
'name': 'robot1',
'x_pose': 0.0,
'y_pose': 0.5,
'z_pose': 0.01
}, {
'name': 'robot2',
'x_pose': 0.0,
'y_pose': -0.5,
'z_pose': 0.01
}]
# Launch Gazebo server for simulation
start_gazebo_cmd = ExecuteProcess(cmd=[
'gzserver', '-s', 'libgazebo_ros_factory.so', '--minimal_comms', world
],
output='screen')
# Define commands for spawing the robots into Gazebo
spawn_robots_cmds = []
for robot in robots:
spawn_robots_cmds.append(
Node(package='nav2_gazebo_spawner',
executable='nav2_gazebo_spawner',
output='screen',
arguments=[
'--robot_name',
TextSubstitution(text=robot['name']), '--robot_namespace',
TextSubstitution(text=robot['name']), '--sdf',
TextSubstitution(text=sdf), '-x',
TextSubstitution(text=str(robot['x_pose'])), '-y',
TextSubstitution(text=str(robot['y_pose'])), '-z',
TextSubstitution(text=str(robot['z_pose']))
]))
# Define commands for launching the robot state publishers
robot_state_pubs_cmds = []
for robot in robots:
robot_state_pubs_cmds.append(
Node(package='robot_state_publisher',
executable='robot_state_publisher',
namespace=TextSubstitution(text=robot['name']),
output='screen',
parameters=[{
'use_sim_time': 'True'
}],
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
arguments=[urdf]))
# Define commands for launching the navigation instances
nav_instances_cmds = []
for robot in robots:
params_file = eval(f"{robot['name']}_params_file")
group = GroupAction([
# Instances use the robot's name for namespace
PushRosNamespace(robot['name']),
IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
launch_arguments={
'namespace': robot['name'],
'map': map_yaml_file,
'use_sim_time': 'True',
'params_file': params_file,
'bt_xml_file': bt_xml_file,
'autostart': 'True',
'use_remappings': 'True'
}.items())
])
nav_instances_cmds.append(group)
ld = LaunchDescription()
ld.add_action(
SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), )
ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), )
ld.add_action(start_gazebo_cmd)
for spawn_robot in spawn_robots_cmds:
ld.add_action(spawn_robot)
for state_pub in robot_state_pubs_cmds:
ld.add_action(state_pub)
for nav_instance in nav_instances_cmds:
ld.add_action(nav_instance)
return ld
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
# Names and poses of the robots
robots = [{
'name': 'robot1',
'x_pose': 0.0,
'y_pose': 0.5,
'z_pose': 0.01
}, {
'name': 'robot2',
'x_pose': 0.0,
'y_pose': -0.5,
'z_pose': 0.01
}]
# Simulation settings
world = LaunchConfiguration('world')
simulator = LaunchConfiguration('simulator')
# On this example all robots are launched with the same settings
map_yaml_file = LaunchConfiguration('map')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
rviz_config_file = LaunchConfiguration('rviz_config')
log_settings = LaunchConfiguration('log_settings', default='true')
# Declare the launch arguments
declare_world_cmd = DeclareLaunchArgument(
'world',
default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'),
description='Full path to world file to load')
declare_simulator_cmd = DeclareLaunchArgument(
'simulator',
default_value='gazebo',
description='The simulator to use (gazebo or gzserver)')
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(bringup_dir, 'maps',
'turtlebot3_world.yaml'),
description='Full path to map file to load')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for all launched nodes')
declare_bt_xml_cmd = DeclareLaunchArgument(
'bt_xml_file',
default_value=os.path.join(
get_package_share_directory('nav2_bt_navigator'), 'behavior_trees',
'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config',
default_value=os.path.join(bringup_dir, 'rviz',
'nav2_namespaced_view.rviz'),
description='Full path to the RVIZ config file to use')
# Start Gazebo with plugin providing the robot spawing service
start_gazebo_cmd = ExecuteProcess(
cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', world],
output='screen')
# Define commands for spawing the robots into Gazebo
spawn_robots_cmds = []
for robot in robots:
spawn_robots_cmds.append(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch',
'spawn_robot_launch.py')),
launch_arguments={
'x_pose': TextSubstitution(text=str(robot['x_pose'])),
'y_pose': TextSubstitution(text=str(robot['y_pose'])),
'z_pose': TextSubstitution(text=str(robot['z_pose'])),
'robot_name': robot['name'],
'turtlebot_type': TextSubstitution(text='waffle')
}.items()))
# Define commands for launching the navigation instances
nav_instances_cmds = []
for robot in robots:
namespaced_rviz_config_file = ReplaceString(
source_file=rviz_config_file,
replacements={'<robot_namespace>': ('/' + robot['name'])})
group = GroupAction([
# TODO(orduno)
# Each `action.Node` within the `localization` and `navigation` launch
# files has two versions, one with the required remaps and another without.
# The `use_remappings` flag specifies which runs.
# A better mechanism would be to have a PushNodeRemapping() action:
# https://github.com/ros2/launch_ros/issues/56
# For more on why we're remapping topics, see the note below
# PushNodeRemapping(remappings)
# Instances use the robot's name for namespace
PushRosNamespace(robot['name']),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch',
'nav2_tb3_simulation_launch.py')),
launch_arguments={
# TODO(orduno) might not be necessary to pass the robot name
'namespace': robot['name'],
'map_yaml_file': map_yaml_file,
'use_sim_time': 'True',
'params_file': params_file,
'bt_xml_file': bt_xml_file,
'autostart': 'False',
'use_remappings': 'True',
'rviz_config_file': namespaced_rviz_config_file,
'use_simulator': 'False'
}.items()),
LogInfo(condition=IfCondition(log_settings),
msg=['Launching ', robot['name']]),
LogInfo(condition=IfCondition(log_settings),
msg=[robot['name'], ' map yaml: ', map_yaml_file]),
LogInfo(condition=IfCondition(log_settings),
msg=[robot['name'], ' params yaml: ', params_file]),
LogInfo(condition=IfCondition(log_settings),
msg=[robot['name'], ' behavior tree xml: ', bt_xml_file]),
LogInfo(condition=IfCondition(log_settings),
msg=[
robot['name'], ' rviz config file: ',
namespaced_rviz_config_file
])
])
nav_instances_cmds.append(group)
# A note on the `remappings` variable defined above and the fact it's passed as a node arg.
# A few topics have fully qualified names (have a leading '/'), these need to be remapped
# to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# for multi-robot transforms:
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
ld.add_action(declare_rviz_config_file_cmd)
# Add the actions to start gazebo, robots and simulations
ld.add_action(start_gazebo_cmd)
for spawn_robot_cmd in spawn_robots_cmds:
ld.add_action(spawn_robot_cmd)
for simulation_instance_cmd in nav_instances_cmds:
ld.add_action(simulation_instance_cmd)
return ld
