diff --git a/rclpy/rclpy/exceptions.py b/rclpy/rclpy/exceptions.py index 3a3bb6a18..f4d33be81 100644 --- a/rclpy/rclpy/exceptions.py +++ b/rclpy/rclpy/exceptions.py @@ -95,6 +95,21 @@ def __init__(self, parameter, *args): Exception.__init__(self, 'Invalid parameter name', parameter, *args) +class InvalidParameterTypeException(ParameterException): + """Raised when a parameter is rejected for having an invalid type.""" + + def __init__(self, desired_parameter, expected_type, *args): + from rclpy.parameter import Parameter + Exception.__init__( + self, + f"Trying to set parameter '{desired_parameter._name}' to '{desired_parameter._value}'" + f" of type '{Parameter.Type.from_parameter_value(desired_parameter._value).name}'" + f", expecting type '{expected_type}'", + *args) + self._actual_type = desired_parameter.type_ + self._param_name = desired_parameter.name + + class InvalidParameterValueException(ParameterException): """Raised when a parameter is rejected by a user callback or when applying a descriptor.""" @@ -112,6 +127,16 @@ def __init__(self, parameter, *args): Exception.__init__(self, 'Attempted to modify read-only parameter', parameter, *args) +class NoParameterOverrideProvidedException(ParameterException): + """Raised when no override is provided for a statically typed parameter with no default.""" + + def __init__(self, parameter_name, *args): + Exception.__init__( + self, + f"No parameter override provided for '{parameter_name}' when one was expected", + *args) + + class ROSInterruptException(Exception): """Raised when an operation is canceled by rclpy shutting down.""" diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index aae21f57d..2b2479235 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -25,6 +25,7 @@ from typing import TypeVar from typing import Union +import warnings import weakref from rcl_interfaces.msg import FloatingPointRange @@ -32,6 +33,7 @@ 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 @@ -43,8 +45,10 @@ from rclpy.clock import ROSClock from rclpy.constants import S_TO_NS from rclpy.context import Context +from rclpy.exceptions import InvalidParameterTypeException from rclpy.exceptions import InvalidParameterValueException from rclpy.exceptions import InvalidTopicNameException +from rclpy.exceptions import NoParameterOverrideProvidedException from rclpy.exceptions import NotInitializedException from rclpy.exceptions import ParameterAlreadyDeclaredException from rclpy.exceptions import ParameterImmutableException @@ -328,7 +332,7 @@ def declare_parameter( self, name: str, value: Any = None, - descriptor: ParameterDescriptor = ParameterDescriptor(), + descriptor: Optional[ParameterDescriptor] = None, ignore_override: bool = False ) -> Parameter: """ @@ -346,13 +350,20 @@ def declare_parameter( :raises: InvalidParameterException if the parameter name is invalid. :raises: InvalidParameterValueException if the registered callback rejects the parameter. """ - return self.declare_parameters('', [(name, value, descriptor)], ignore_override)[0] + if value is None and descriptor is None: + # Temporal patch so we get deprecation warning if only a name is provided. + args = (name, ) + else: + descriptor = ParameterDescriptor() if descriptor is None else descriptor + args = (name, value, descriptor) + return self.declare_parameters('', [args], ignore_override)[0] def declare_parameters( self, namespace: str, parameters: List[Union[ Tuple[str], + Tuple[str, Parameter.Type], Tuple[str, Any], Tuple[str, Any, ParameterDescriptor], ]], @@ -401,42 +412,66 @@ def declare_parameters( ) value = None - descriptor = ParameterDescriptor() + param_type = None # Get the values from the tuple, checking its types. # Use defaults if the tuple doesn't contain value and / or descriptor. - try: - name = parameter_tuple[0] - assert \ - isinstance(name, str), \ - ( - 'First element {name} at index {index} in parameters list ' - 'is not a str.'.format_map(locals()) - ) - - # Get value from parameter overrides, of from tuple if it doesn't exist. - if not ignore_override and name in self._parameter_overrides: - value = self._parameter_overrides[name].value - else: - # This raises a TypeError if it's not possible to get a type from the tuple. - value = parameter_tuple[1] - - # Get descriptor from tuple. - descriptor = parameter_tuple[2] - assert \ - isinstance(descriptor, ParameterDescriptor), \ - ( - 'Third element {descriptor} at index {index} in parameters list ' - 'is not a ParameterDescriptor.'.format_map(locals()) - ) - except AssertionError as assertion_error: - raise TypeError(assertion_error) - except IndexError: - # This means either value or descriptor were not defined which is fine. - pass + name = parameter_tuple[0] + second_arg = parameter_tuple[1] if 1 < len(parameter_tuple) else None + descriptor = parameter_tuple[2] if 2 < len(parameter_tuple) else ParameterDescriptor() + + if not isinstance(name, str): + raise TypeError( + f'First element {name} at index {index} in parameters list ' + 'is not a str.') + if not isinstance(descriptor, ParameterDescriptor): + raise TypeError( + f'Third element {descriptor} at index {index} in parameters list ' + 'is not a ParameterDescriptor.' + ) + + if len(parameter_tuple) == 1: + warnings.warn( + f"when declaring parmater named '{name}', " + 'declaring a parameter only providing its name is deprecated. ' + 'You have to either:\n' + '\t- Pass a name and a default value different to "PARAMETER NOT SET"' + ' (and optionally a descriptor).\n' + '\t- Pass a name and a parameter type.\n' + '\t- Pass a name and a descriptor with `dynamic_typing=True') + descriptor.dynamic_typing = True + + if isinstance(second_arg, Parameter.Type): + if second_arg.value == Parameter.Type.NOT_SET: + raise ValueError( + f'Cannot declare parameter {{{name}}} as statically typed of type NOT_SET') + if descriptor.dynamic_typing is True: + raise ValueError( + f'When declaring parameter {{{name}}} passing a descriptor with' + '`dynamic_typing=True` is not allowed when the parameter type is provided') + descriptor.type = second_arg.value + else: + value = second_arg + if not descriptor.dynamic_typing and value is not None: + # infer type from default value + if not isinstance(value, ParameterValue): + descriptor.type = Parameter.Type.from_parameter_value(value).value + else: + if value.type == ParameterType.PARAMETER_NOT_SET: + raise ValueError( + 'Cannot declare a statically typed parameter with default value ' + 'of type PARAMETER_NOT_SET') + descriptor.type = value.type + + # Get value from parameter overrides, of from tuple if it doesn't exist. + if not ignore_override and name in self._parameter_overrides: + value = self._parameter_overrides[name].value + + if value is None and not descriptor.dynamic_typing: + raise NoParameterOverrideProvidedException(name) if namespace: - name = '{namespace}.{name}'.format_map(locals()) + name = f'{namespace}.{name}' # Note(jubeira): declare_parameters verifies the name, but set_parameters doesn't. validate_parameter_name(name) @@ -643,6 +678,9 @@ def _set_parameters( allow_not_set_type=allow_undeclared_parameters ) if raise_on_failure and not result.successful: + if result.reason.startswith('Wrong parameter type'): + raise InvalidParameterTypeException( + param, Parameter.Type(descriptors[param._name].type).name) raise InvalidParameterValueException(param.name, param.value, result.reason) results.append(result) return results @@ -858,14 +896,22 @@ def _apply_descriptor( else: descriptor.name = parameter.name - # The type in the descriptor has to match the type of the parameter. - descriptor.type = parameter.type_.value - if check_read_only and descriptor.read_only: return SetParametersResult( successful=False, reason='Trying to set a read-only parameter: {}.'.format(parameter.name)) + if descriptor.dynamic_typing or self._allow_undeclared_parameters: + descriptor.type = parameter.type_.value + elif descriptor.type != parameter.type_.value: + return SetParametersResult( + successful=False, + reason=( + 'Wrong parameter type, expected ' + f"'{Parameter.Type(descriptor.type)}'" + f" got '{parameter.type_}'") + ) + if parameter.type_ == Parameter.Type.INTEGER and descriptor.integer_range: return self._apply_integer_range(parameter, descriptor.integer_range[0]) diff --git a/rclpy/test/resources/test_node/test_parameters.yaml b/rclpy/test/resources/test_node/test_parameters.yaml index a79d4fb57..a27cfb619 100644 --- a/rclpy/test/resources/test_node/test_parameters.yaml +++ b/rclpy/test/resources/test_node/test_parameters.yaml @@ -5,4 +5,4 @@ initial_foobar: False /my_ns/my_node: ros__parameters: - initial_fizz: 23 + initial_fizz: param_file_override diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index 601750e0e..71e9b3202 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -16,6 +16,9 @@ import time import unittest from unittest.mock import Mock +import warnings + +import pytest from rcl_interfaces.msg import FloatingPointRange from rcl_interfaces.msg import IntegerRange @@ -28,9 +31,11 @@ from rclpy.clock import ClockType from rclpy.duration import Duration from rclpy.exceptions import InvalidParameterException +from rclpy.exceptions import InvalidParameterTypeException from rclpy.exceptions import InvalidParameterValueException from rclpy.exceptions import InvalidServiceNameException from rclpy.exceptions import InvalidTopicNameException +from rclpy.exceptions import NoParameterOverrideProvidedException from rclpy.exceptions import ParameterAlreadyDeclaredException from rclpy.exceptions import ParameterImmutableException from rclpy.exceptions import ParameterNotDeclaredException @@ -481,7 +486,9 @@ def setUp(self): parameter_overrides=[ Parameter('initial_foo', Parameter.Type.INTEGER, 4321), Parameter('initial_bar', Parameter.Type.STRING, 'init_param'), - Parameter('initial_baz', Parameter.Type.DOUBLE, 3.14) + Parameter('initial_baz', Parameter.Type.DOUBLE, 3.14), + Parameter('initial_decl_with_type', Parameter.Type.DOUBLE, 3.14), + Parameter('initial_decl_wrong_type', Parameter.Type.DOUBLE, 3.14), ], cli_args=[ '--ros-args', '-p', 'initial_fizz:=buzz', @@ -497,18 +504,23 @@ def tearDown(self): rclpy.shutdown(context=self.context) def test_declare_parameter(self): + with pytest.raises(ValueError): + result_initial_foo = self.node.declare_parameter( + 'initial_foo', ParameterValue(), ParameterDescriptor()) + dynamic_typing_descriptor = ParameterDescriptor() + dynamic_typing_descriptor.dynamic_typing = True result_initial_foo = self.node.declare_parameter( - 'initial_foo', ParameterValue(), ParameterDescriptor()) + 'initial_foo', ParameterValue(), dynamic_typing_descriptor) result_initial_bar = self.node.declare_parameter( 'initial_bar', 'ignoring_override', ParameterDescriptor(), ignore_override=True) result_initial_fizz = self.node.declare_parameter( 'initial_fizz', 'default', ParameterDescriptor()) result_initial_baz = self.node.declare_parameter( - 'initial_baz', ParameterValue(), ParameterDescriptor()) + 'initial_baz', 0., ParameterDescriptor()) result_initial_buzz = self.node.declare_parameter( - 'initial_buzz', ParameterValue(), ParameterDescriptor()) + 'initial_buzz', 0., ParameterDescriptor()) result_initial_foobar = self.node.declare_parameter( - 'initial_foobar', ParameterValue(), ParameterDescriptor()) + 'initial_foobar', True, ParameterDescriptor()) result_foo = self.node.declare_parameter( 'foo', 42, ParameterDescriptor()) @@ -516,7 +528,11 @@ def test_declare_parameter(self): 'bar', 'hello', ParameterDescriptor()) result_baz = self.node.declare_parameter( 'baz', 2.41, ParameterDescriptor()) - result_value_not_set = self.node.declare_parameter('value_not_set') + with warnings.catch_warnings(record=True) as w: + warnings.simplefilter('always', category=UserWarning) + result_value_not_set = self.node.declare_parameter('value_not_set') + assert len(w) == 1 + assert issubclass(w[0].category, UserWarning) # OK cases. self.assertIsInstance(result_initial_foo, Parameter) @@ -530,9 +546,10 @@ def test_declare_parameter(self): # initial_foo and initial_fizz get override values; initial_bar does not. self.assertEqual(result_initial_foo.value, 4321) self.assertEqual(result_initial_bar.value, 'ignoring_override') - self.assertEqual(result_initial_fizz.value, 23) # provided by CLI, overridden by file + # provided by CLI, overridden by file + self.assertEqual(result_initial_fizz.value, 'param_file_override') self.assertEqual(result_initial_baz.value, 3.14) # provided by file, overridden manually - self.assertEqual(result_initial_buzz.value, 1) # provided by CLI + self.assertEqual(result_initial_buzz.value, 1.) # provided by CLI self.assertEqual(result_initial_foobar.value, False) # provided by file self.assertEqual(result_foo.value, 42) self.assertEqual(result_bar.value, 'hello') @@ -540,7 +557,7 @@ def test_declare_parameter(self): self.assertIsNone(result_value_not_set.value) self.assertEqual(self.node.get_parameter('initial_foo').value, 4321) self.assertEqual(self.node.get_parameter('initial_bar').value, 'ignoring_override') - self.assertEqual(self.node.get_parameter('initial_fizz').value, 23) + self.assertEqual(self.node.get_parameter('initial_fizz').value, 'param_file_override') self.assertEqual(self.node.get_parameter('initial_baz').value, 3.14) self.assertEqual(self.node.get_parameter('initial_buzz').value, 1) self.assertEqual(self.node.get_parameter('initial_foobar').value, False) @@ -574,7 +591,7 @@ def test_declare_parameter(self): 'wrong_name_type', ParameterDescriptor()) - with self.assertRaises(TypeError): + with self.assertRaises(ValueError): self.node.declare_parameter( 'wrong_parameter_value_type', ParameterValue(), ParameterDescriptor()) @@ -582,6 +599,12 @@ def test_declare_parameter(self): self.node.declare_parameter( 'wrong_parameter_descriptor_type', 1, ParameterValue()) + descriptor = ParameterDescriptor() + descriptor.dynamic_typing = True + with self.assertRaises(ValueError): + self.node.declare_parameter( + 'dynamic_typing_and_static_type', Parameter.Type.DOUBLE, descriptor=descriptor) + def test_declare_parameters(self): parameters = [ ('initial_foo', 0, ParameterDescriptor()), @@ -591,7 +614,21 @@ def test_declare_parameters(self): ('value_not_set',) ] - result = self.node.declare_parameters('', parameters) + with pytest.raises(NoParameterOverrideProvidedException): + self.node.declare_parameter('no_override', Parameter.Type.INTEGER) + + with pytest.raises(InvalidParameterTypeException): + self.node.declare_parameter('initial_decl_wrong_type', Parameter.Type.INTEGER) + + self.assertAlmostEqual( + self.node.declare_parameter('initial_decl_with_type', Parameter.Type.DOUBLE).value, + 3.14) + + with warnings.catch_warnings(record=True) as w: + warnings.simplefilter('always', category=UserWarning) + result = self.node.declare_parameters('', parameters) + assert len(w) == 1 + assert issubclass(w[0].category, UserWarning) # OK cases - using overrides. self.assertIsInstance(result, list) @@ -613,7 +650,10 @@ def test_declare_parameters(self): self.assertIsNone(self.node.get_parameter('value_not_set').value) self.assertTrue(self.node.has_parameter('value_not_set')) - result = self.node.declare_parameters('namespace', parameters) + with warnings.catch_warnings(record=True) as w: + warnings.simplefilter('always', category=UserWarning) + result = self.node.declare_parameters('namespace', parameters) + assert len(w) == 1 # OK cases. self.assertIsInstance(result, list) @@ -695,7 +735,7 @@ def test_declare_parameters(self): )] ) - with self.assertRaises(TypeError): + with self.assertRaises(ValueError): self.node.declare_parameters( '', [( @@ -1175,6 +1215,8 @@ def test_node_set_parameters_read_only(self): self.assertEqual(self.node.get_parameter('immutable_baz').value, 2.41) def test_node_set_parameters_implicit_undeclare(self): + dynamic_typing_descriptor = ParameterDescriptor() + dynamic_typing_descriptor.dynamic_typing = True parameter_tuples = [ ( 'foo', @@ -1184,7 +1226,7 @@ def test_node_set_parameters_implicit_undeclare(self): ( 'bar', 'hello', - ParameterDescriptor() + dynamic_typing_descriptor ), ( 'baz', @@ -1392,6 +1434,8 @@ def test_node_set_parameters_atomically_read_only(self): self.assertEqual(self.node.get_parameter('immutable_baz').value, 2.41) def test_node_set_parameters_atomically_implicit_undeclare(self): + dynamic_typing_descriptor = ParameterDescriptor() + dynamic_typing_descriptor.dynamic_typing = True parameter_tuples = [ ( 'foo', @@ -1401,7 +1445,7 @@ def test_node_set_parameters_atomically_implicit_undeclare(self): ( 'bar', 'hello', - ParameterDescriptor() + dynamic_typing_descriptor ), ( 'baz', @@ -1418,7 +1462,9 @@ def test_node_set_parameters_atomically_implicit_undeclare(self): self.assertEqual(self.node.get_parameter('baz').value, 2.41) # Now undeclare one of them implicitly. - self.node.set_parameters_atomically([Parameter('bar', Parameter.Type.NOT_SET, None)]) + result = self.node.set_parameters_atomically([ + Parameter('bar', Parameter.Type.NOT_SET, None)]) + self.assertEqual(result.successful, True) self.assertEqual(self.node.get_parameter('foo').value, 42) self.assertFalse(self.node.has_parameter('bar')) self.assertEqual(self.node.get_parameter('baz').value, 2.41) @@ -1540,7 +1586,8 @@ def test_set_descriptor(self): type=ParameterType.PARAMETER_INTEGER, # Type will be ignored too. additional_constraints='some constraints', read_only=False, - integer_range=[IntegerRange(from_value=-10, to_value=10, step=2)] + integer_range=[IntegerRange(from_value=-10, to_value=10, step=2)], + dynamic_typing=True ) ) self.assertEqual(value.type, Parameter.Type.STRING.value) @@ -1650,13 +1697,6 @@ def test_floating_point_range_descriptor(self): self.assertFalse(result[0].successful) self.assertEqual(self.node.get_parameter('in_range').value, 4.5) - # Change in_range parameter to int; ranges will not apply. - result = self.node.set_parameters([Parameter('in_range', value=12)]) - self.assertIsInstance(result, list) - self.assertIsInstance(result[0], SetParametersResult) - self.assertTrue(result[0].successful) - self.assertEqual(self.node.get_parameter('in_range').value, 12) - # From and to are always valid. # Parameters that don't comply with the description will raise an exception. fp_range = FloatingPointRange(from_value=-10.0, to_value=0.0, step=30.0) @@ -1739,13 +1779,6 @@ def test_integer_range_descriptor(self): self.assertFalse(result[0].successful) self.assertEqual(self.node.get_parameter('in_range').value, 4) - # Change in_range parameter to a float; ranges will not apply. - result = self.node.set_parameters([Parameter('in_range', value=12.0)]) - self.assertIsInstance(result, list) - self.assertIsInstance(result[0], SetParametersResult) - self.assertTrue(result[0].successful) - self.assertAlmostEqual(self.node.get_parameter('in_range').value, 12.0) - # From and to are always valid. # Parameters that don't comply with the description will raise an exception. integer_range = IntegerRange(from_value=-10, to_value=0, step=30) @@ -1784,6 +1817,36 @@ def test_integer_range_descriptor(self): self.assertEqual(self.node.get_parameter('to_value_no_step').value, 10) self.assertEqual(self.node.get_parameter('in_range_no_step').value, 5) + def test_static_dynamic_typing(self): + dynamic_typing_descriptor = ParameterDescriptor() + dynamic_typing_descriptor.dynamic_typing = True + parameters = [ + ('int_param', 0), + ('dynamic_param', None, dynamic_typing_descriptor), + ] + result = self.node.declare_parameters('', parameters) + + result = self.node.set_parameters([Parameter('int_param', value='asd')])[0] + self.assertFalse(result.successful) + self.assertTrue(result.reason.startswith('Wrong parameter type')) + + self.assertTrue(self.node.set_parameters([Parameter('int_param', value=3)])[0].successful) + + self.assertTrue( + self.node.set_parameters([Parameter('dynamic_param', value='asd')])[0].successful) + self.assertTrue( + self.node.set_parameters([Parameter('dynamic_param', value=3)])[0].successful) + + result = self.node.set_parameters_atomically([ + Parameter('dynamic_param', value=3), Parameter('int_param', value='asd')]) + self.assertFalse(result.successful) + self.assertTrue(result.reason.startswith('Wrong parameter type')) + + self.assertTrue(self.node.set_parameters_atomically([ + Parameter('dynamic_param', value=None), Parameter('int_param', value=4)]).successful) + self.assertEqual(self.node.get_parameter('int_param').value, 4) + self.assertFalse(self.node.has_parameter('dynamic_param')) + class TestCreateNode(unittest.TestCase):