Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add list_parameters & test #1124

Merged
merged 3 commits into from Sep 8, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
62 changes: 62 additions & 0 deletions rclpy/rclpy/node.py
Expand Up @@ -32,12 +32,14 @@

from rcl_interfaces.msg import FloatingPointRange
from rcl_interfaces.msg import IntegerRange
from rcl_interfaces.msg import ListParametersResult
from rcl_interfaces.msg import Parameter as ParameterMsg
from rcl_interfaces.msg import ParameterDescriptor
from rcl_interfaces.msg import ParameterEvent
from rcl_interfaces.msg import ParameterType
from rcl_interfaces.msg import ParameterValue
from rcl_interfaces.msg import SetParametersResult
from rcl_interfaces.srv import ListParameters

from rclpy.callback_groups import CallbackGroup
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
Expand Down Expand Up @@ -956,6 +958,66 @@ def _set_parameters_atomically_common(

return result

def list_parameters(
self,
prefixes: List[str],
depth: int
) -> ListParametersResult:
"""
Get a list of parameter names and their prefixes.

:param prefixes: A list of prefixes to filter the parameter names.
Only the parameter names that start with any of the prefixes are returned.
If empty, all parameter names are returned.
:param depth: The depth of nested parameter names to include.
:return: The list of parameter names and their prefixes.
:raises: TypeError if the type of any of the passed arguments is not an expected type.
:raises: ValueError if depth is a negative integer.
"""
if not isinstance(prefixes, list):
raise TypeError('The prefixes argument must be a list')
if not all(isinstance(prefix, str) for prefix in prefixes):
raise TypeError('All prefixes must be instances of type str')
if not isinstance(depth, int):
raise TypeError('The depth must be instance of type int')
if depth < 0:
raise ValueError('The depth must be greater than or equal to zero')

result = ListParametersResult()

separator_less_than_depth: Callable[[str], bool] = \
lambda str: str.count(PARAMETER_SEPARATOR_STRING) < depth

recursive: bool = \
(len(prefixes) == 0) and (depth == ListParameters.Request.DEPTH_RECURSIVE)

for param_name in self._parameters.keys():
if not recursive:
get_all: bool = (len(prefixes) == 0) and (separator_less_than_depth(param_name))
if not get_all:
prefix_matches = any(
param_name == prefix or
(
param_name.startswith(prefix+PARAMETER_SEPARATOR_STRING) and
(
depth == ListParameters.Request.DEPTH_RECURSIVE or
separator_less_than_depth(param_name[len(prefix)+1:])
clalancette marked this conversation as resolved.
Show resolved Hide resolved
)
)
for prefix in prefixes
)
if not prefix_matches:
continue

result.names.append(param_name)
last_separator = param_name.rfind(PARAMETER_SEPARATOR_STRING)
if last_separator != -1:
prefix = param_name[:last_separator]
if prefix not in result.prefixes:
result.prefixes.append(prefix)

return result

def _check_undeclared_parameters(self, parameter_list: List[Parameter]):
"""
Check if parameter list has correct types and was declared beforehand.
Expand Down
149 changes: 149 additions & 0 deletions rclpy/test/test_node.py
Expand Up @@ -24,6 +24,7 @@

from rcl_interfaces.msg import FloatingPointRange
from rcl_interfaces.msg import IntegerRange
from rcl_interfaces.msg import ListParametersResult
from rcl_interfaces.msg import ParameterDescriptor
from rcl_interfaces.msg import ParameterType
from rcl_interfaces.msg import ParameterValue
Expand Down Expand Up @@ -1155,6 +1156,154 @@ def test_node_set_parameters_rejection_list(self):
self.assertIsInstance(result[2], SetParametersResult)
self.assertTrue(result[2].successful)

def test_node_list_parameters(self):
parameters = [
('foo_prefix.foo', 43),
('foo_prefix.bar', 'hello'),
('foo_prefix.baz', 2.41),
('bar_prefix.foo', 1),
('bar_prefix.bar', 12.3),
('bar_prefix.baz', 'world'),
]
self.node.declare_parameters('', parameters)

param_result = self.node.list_parameters([], 0)
self.assertIsInstance(param_result, ListParametersResult)
# Currently the default parameters are 'use_sim_time' and 'start_type_description_service'.
# Those may change. e.g) QoS override parameters
self.assertEqual(len(param_result.names), len(parameters)+2)
self.assertCountEqual(
param_result.names,
[
'foo_prefix.foo',
'foo_prefix.bar',
'foo_prefix.baz',
'bar_prefix.foo',
'bar_prefix.bar',
'bar_prefix.baz',
USE_SIM_TIME_NAME,
START_TYPE_DESCRIPTION_SERVICE_PARAM
]
)
self.assertEqual(len(param_result.prefixes), 2)
self.assertCountEqual(
param_result.prefixes,
[
'foo_prefix',
'bar_prefix',
]
)

param_result = self.node.list_parameters(
prefixes=['foo_prefix', 'bar_prefix'], depth=0)
self.assertIsInstance(param_result, ListParametersResult)
self.assertEqual(len(param_result.names), len(parameters))
self.assertCountEqual(
param_result.names,
[
'foo_prefix.foo',
'foo_prefix.bar',
'foo_prefix.baz',
'bar_prefix.foo',
'bar_prefix.bar',
'bar_prefix.baz'
]
)
self.assertEqual(len(param_result.prefixes), 2)
self.assertCountEqual(
param_result.prefixes,
[
'foo_prefix',
'bar_prefix',
]
)

param_result = self.node.list_parameters(
prefixes=['foo_prefix'], depth=0)
self.assertIsInstance(param_result, ListParametersResult)
self.assertEqual(len(param_result.names), 3)
self.assertCountEqual(
param_result.names,
[
'foo_prefix.foo',
'foo_prefix.bar',
'foo_prefix.baz'
]
)
self.assertEqual(len(param_result.prefixes), 1)
self.assertCountEqual(
param_result.prefixes,
[
'foo_prefix'
]
)

param_result = self.node.list_parameters(prefixes=[], depth=1)
self.assertIsInstance(param_result, ListParametersResult)
self.assertEqual(len(param_result.names), 2)
self.assertCountEqual(
param_result.names, [USE_SIM_TIME_NAME, START_TYPE_DESCRIPTION_SERVICE_PARAM]
)
self.assertEqual(len(param_result.prefixes), 0)
self.assertCountEqual(param_result.prefixes, [])

param_result = self.node.list_parameters(prefixes=[], depth=2)
self.assertIsInstance(param_result, ListParametersResult)
self.assertEqual(len(param_result.names), len(parameters)+2)
self.assertCountEqual(
param_result.names,
[
'foo_prefix.foo',
'foo_prefix.bar',
'foo_prefix.baz',
'bar_prefix.foo',
'bar_prefix.bar',
'bar_prefix.baz',
USE_SIM_TIME_NAME,
START_TYPE_DESCRIPTION_SERVICE_PARAM
]
)
self.assertEqual(len(param_result.prefixes), 2)
self.assertCountEqual(
param_result.prefixes,
[
'foo_prefix',
'bar_prefix',
]
)

param_result = self.node.list_parameters(prefixes=['foo_prefix', 'bar_prefix'], depth=1)
self.assertIsInstance(param_result, ListParametersResult)
self.assertEqual(len(param_result.names), len(parameters))
self.assertCountEqual(
param_result.names,
[
'foo_prefix.foo',
'foo_prefix.bar',
'foo_prefix.baz',
'bar_prefix.foo',
'bar_prefix.bar',
'bar_prefix.baz',
]
)
self.assertEqual(len(param_result.prefixes), 2)
self.assertCountEqual(
param_result.prefixes,
[
'foo_prefix',
'bar_prefix',
]
)

with self.assertRaises(TypeError):
self.node.list_parameters(prefixes='foo', depth=0)

with self.assertRaises(ValueError):
self.node.list_parameters(prefixes=[], depth=-1)

with self.assertRaises(TypeError):
self.node.list_parameters(prefixes=[], depth=1.5)

def modify_parameter_callback(self, parameter_list: List[Parameter]):
modified_list = parameter_list.copy()
for param in parameter_list:
Expand Down