From 7159de6ccecf2cd49f9bb0242cb8a07eb7e3d421 Mon Sep 17 00:00:00 2001 From: Juan Ignacio Ubeira Date: Mon, 13 May 2019 11:22:00 -0300 Subject: [PATCH 1/2] Parameter features enhancement. - Adding set_descriptor. - Using better optional parameters for declare_parameters. - Handling read-only parameters. Signed-off-by: Juan Ignacio Ubeira --- rclpy/rclpy/exceptions.py | 2 +- rclpy/rclpy/node.py | 180 ++++++++++++++++++++++++++++++++------ 2 files changed, 156 insertions(+), 26 deletions(-) diff --git a/rclpy/rclpy/exceptions.py b/rclpy/rclpy/exceptions.py index 3da4f04a4..80c641d3f 100644 --- a/rclpy/rclpy/exceptions.py +++ b/rclpy/rclpy/exceptions.py @@ -96,7 +96,7 @@ def __init__(self, parameter, *args): class InvalidParameterValueException(ParameterException): - """Raised when a parameter to be declared is rejected by a callback.""" + """Raised when a parameter is rejected by a user callback or when applying a descriptor.""" def __init__(self, parameter, value, *args): Exception.__init__( diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index d46b8e5c4..b3069b50d 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -18,6 +18,7 @@ from typing import Optional from typing import Tuple from typing import TypeVar +from typing import Union import weakref @@ -308,7 +309,11 @@ def declare_parameter( def declare_parameters( self, namespace: str, - parameters: List[Tuple[str, Optional[ParameterValue], Optional[ParameterDescriptor]]] + parameters: List[Union[ + Tuple[str], + Tuple[str, ParameterValue], + Tuple[str, ParameterValue, ParameterDescriptor], + ]] ) -> List[Parameter]: """ Declare a list of parameters. @@ -325,24 +330,56 @@ def declare_parameters( :raises: ParameterAlreadyDeclaredException if the parameter had already been declared. :raises: InvalidParameterException if the parameter name is invalid. :raises: InvalidParameterValueException if the registered callback rejects any parameter. + :raises: TypeError if any tuple in :param:`parameters` does not match the annotated type. """ parameter_list = [] descriptor_list = [] - for parameter_tuple in parameters: - name = parameter_tuple[0] - assert isinstance(name, str) - # Get value from initial parameters, of from tuple if it doesn't exist. - if name in self._initial_parameters: - value = self._initial_parameters[name].get_parameter_value() - elif parameter_tuple[1] is None: - value = ParameterValue() - else: - value = parameter_tuple[1] - assert isinstance(value, ParameterValue) - descriptor = parameter_tuple[2] - if descriptor is None: - descriptor = ParameterDescriptor() - assert isinstance(descriptor, ParameterDescriptor) + for index, parameter_tuple in enumerate(parameters): + if len(parameter_tuple) < 1 or len(parameter_tuple) > 3: + raise TypeError( + 'Invalid parameter tuple length at index {index} in parameters list: ' + '{parameter_tuple}; expecting length between 1 and 3'.format_map(locals()) + ) + + value = ParameterValue() + descriptor = ParameterDescriptor() + + # 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 initial parameters, of from tuple if it doesn't exist. + if name in self._initial_parameters: + value = self._initial_parameters[name].get_parameter_value() + else: + value = parameter_tuple[1] + assert \ + isinstance(value, ParameterValue), \ + ( + 'Second element {value} at index {index} in parameters list ' + 'is not a ParameterValue.'.format_map(locals()) + ) + + # 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 # Note(jubeira): declare_parameters verifies the name, but set_parameters doesn't. full_name = namespace + name @@ -468,14 +505,14 @@ def set_parameters(self, parameter_list: List[Parameter]) -> List[SetParametersR :raises: ParameterNotDeclaredException if undeclared parameters are not allowed, and at least one parameter in the list hadn't been declared beforehand. """ - self._check_undeclared_parameters(parameter_list) - return self._set_parameters(parameter_list) + return self._set_parameters(parameter_list, check_undeclared_parameters=True) def _set_parameters( self, parameter_list: List[Parameter], descriptor_list: Optional[List[ParameterDescriptor]] = None, - raise_on_failure=False + raise_on_failure=False, + check_undeclared_parameters=False ) -> List[SetParametersResult]: """ Set parameters for the node, and return the result for the set action. @@ -491,6 +528,12 @@ def _set_parameters( according to `raise_on_failure` flag. :param parameter_list: List of parameters to set. + :param descriptor_list: List of descriptors to set to the given parameters. + If a list is given, it must have the same size as the parameter list. + :param raise_on_failure: True if InvalidParameterValueException has to be raised when + the user callback rejects a parameter, False otherwise. + :param check_undeclared_parameters: True if the method needs to check for undeclared + parameters for each of the elements in the parameter list, False otherwise. :return: The result for each set action as a list. :raises: InvalidParameterValueException if the user-defined callback rejects the parameter value and raise_on_failure flag is True. @@ -500,12 +543,15 @@ def _set_parameters( results = [] for index, param in enumerate(parameter_list): - result = self._set_parameters_atomically([param]) + if check_undeclared_parameters: + self._check_undeclared_parameters([param]) + result = self._set_parameters_atomically( + [param], + None if descriptor_list is None else [descriptor_list[index]] + ) if raise_on_failure and not result.successful: raise InvalidParameterValueException(param.name, param.value) results.append(result) - if descriptor_list is not None: - self._descriptors[param.name] = descriptor_list[index] return results def set_parameters_atomically(self, parameter_list: List[Parameter]) -> SetParametersResult: @@ -555,7 +601,11 @@ def _check_undeclared_parameters(self, parameter_list: List[Parameter]): if (not self._allow_undeclared_parameters and any(undeclared_parameters)): raise ParameterNotDeclaredException(list(undeclared_parameters)) - def _set_parameters_atomically(self, parameter_list: List[Parameter]) -> SetParametersResult: + def _set_parameters_atomically( + self, + parameter_list: List[Parameter], + descriptor_list: Optional[List[ParameterDescriptor]] = None + ) -> SetParametersResult: """ Set the given parameters, all at one time, and then aggregate result. @@ -575,6 +625,9 @@ def _set_parameters_atomically(self, parameter_list: List[Parameter]) -> SetPara :param parameter_list: The list of parameters to set. :return: Aggregate result of setting all the parameters atomically. """ + if descriptor_list is not None: + assert len(descriptor_list) == len(parameter_list) + result = None if self._parameters_callback: result = self._parameters_callback(parameter_list) @@ -589,7 +642,7 @@ def _set_parameters_atomically(self, parameter_list: List[Parameter]) -> SetPara else: parameter_event.node = self.get_namespace() + '/' + self.get_name() - for param in parameter_list: + for index, param in enumerate(parameter_list): if Parameter.Type.NOT_SET == param.type_: if Parameter.Type.NOT_SET != self.get_parameter_or(param.name).type_: # Parameter deleted. (Parameter had value and new value is not set) @@ -602,19 +655,50 @@ def _set_parameters_atomically(self, parameter_list: List[Parameter]) -> SetPara if param.name in self._descriptors: del self._descriptors[param.name] else: + # Update descriptors; set a default if it doesn't exist. + # Don't update if it already exists for the current parameter and a new one + # was not specified in this method call. + if descriptor_list is not None: + self._descriptors[param.name] = descriptor_list[index] + elif param.name not in self._descriptors: + self._descriptors[param.name] = ParameterDescriptor() + if Parameter.Type.NOT_SET == self.get_parameter_or(param.name).type_: # Parameter is new. (Parameter had no value and new value is set) parameter_event.new_parameters.append(param.to_parameter_msg()) else: # Parameter changed. (Parameter had a value and new value is set) + # If a read-only parameter is being modified, stop here. + if self.describe_parameter(param.name).read_only: + result = SetParametersResult( + successful=False, + reason='Parameter {} cannot be set because it is read-only'.format( + param.name) + ) + return result parameter_event.changed_parameters.append( param.to_parameter_msg()) - self._parameters[param.name] = param + + self._apply_descriptor_and_set(param) + parameter_event.stamp = self._clock.now().to_msg() self._parameter_event_publisher.publish(parameter_event) return result + def _apply_descriptor_and_set( + self, + parameter: Parameter, + descriptor: Optional[ParameterDescriptor] = None + ) -> bool: + """Apply parameter descriptor and set parameter.""" + # TODO(jubeira): this is where the parameter ranges should be applied in the future. + if descriptor is None: + descriptor = self._descriptors[parameter.name] + + self._parameters[parameter.name] = parameter + return True + def describe_parameter(self, name: str) -> ParameterDescriptor: """ Get the parameter descriptor of a given parameter. @@ -651,6 +735,52 @@ def describe_parameters(self, names: List[str]) -> List[ParameterDescriptor]: return parameter_descriptors + def set_descriptor( + self, + name: str, + descriptor: ParameterDescriptor, + alternative_value: Optional[ParameterValue] = None + ) -> ParameterValue: + """ + Set a new descriptor for a given parameter. + + :param name: Fully-qualified name of the parameter to set the descriptor to. + :param descriptor: New descriptor to apply to the parameter. + :param alternative_value: Value to set to the parameter if the existing value does not + comply with the new descriptor. + :return: ParameterValue for the given parameter name after applying the new descriptor. + :raises: ParameterNotDeclaredException if parameter had not been declared before + and undeclared parameters are not allowed. + :raises: ParameterImmutableException if the parameter exists and is read-only. + :raises: ParameterValueException if neither the existing value nor the alternative value + complies with the provided descriptor. + """ + if not self.has_parameter(name): + if not self._allow_undeclared_parameters: + raise ParameterNotDeclaredException(name) + else: + return self.get_parameter(name).get_parameter_value() + + if self.describe_parameter(name).read_only: + raise ParameterImmutableException(name) + + current_parameter = self.get_parameter(name) + if alternative_value is None: + alternative_parameter = current_parameter + else: + alternative_parameter = Parameter.from_parameter_msg( + ParameterMsg(name=name, value=alternative_value)) + + # First try keeping the parameter, then try the alternative one. + if ( + self._apply_descriptor_and_set(current_parameter, descriptor) or + self._apply_descriptor_and_set(alternative_parameter, descriptor) + ): + self._descriptors[name] = descriptor + return self.get_parameter(name).get_parameter_value() + else: + raise InvalidParameterValueException(name) + def set_parameters_callback( self, callback: Callable[[List[Parameter]], SetParametersResult] From b86f2fa4687756292fc215c318afae4146749006 Mon Sep 17 00:00:00 2001 From: Juan Ignacio Ubeira Date: Mon, 13 May 2019 18:14:32 -0300 Subject: [PATCH 2/2] Updating test cases for new node parameter API. Signed-off-by: Juan Ignacio Ubeira --- rclpy/test/test_node.py | 588 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 582 insertions(+), 6 deletions(-) diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index 92b08d318..3a90fcb73 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -15,7 +15,11 @@ import unittest from unittest.mock import Mock +from rcl_interfaces.msg import FloatingPointRange +from rcl_interfaces.msg import IntegerRange +from rcl_interfaces.msg import Parameter as ParameterMsg from rcl_interfaces.msg import ParameterDescriptor +from rcl_interfaces.msg import ParameterType from rcl_interfaces.msg import ParameterValue from rcl_interfaces.msg import SetParametersResult from rcl_interfaces.srv import GetParameters @@ -26,6 +30,7 @@ from rclpy.exceptions import InvalidServiceNameException from rclpy.exceptions import InvalidTopicNameException from rclpy.exceptions import ParameterAlreadyDeclaredException +from rclpy.exceptions import ParameterImmutableException from rclpy.exceptions import ParameterNotDeclaredException from rclpy.executors import SingleThreadedExecutor from rclpy.parameter import Parameter @@ -246,6 +251,23 @@ def test_node_set_parameters_atomically(self): self.assertIsInstance(result, SetParametersResult) self.assertTrue(result.successful) + def test_describe_undeclared_parameter(self): + self.assertFalse(self.node.has_parameter('foo')) + + descriptor = self.node.describe_parameter('foo') + self.assertEqual(descriptor, ParameterDescriptor()) + + def test_describe_undeclared_parameters(self): + self.assertFalse(self.node.has_parameter('foo')) + self.assertFalse(self.node.has_parameter('bar')) + + # Check list. + descriptor_list = self.node.describe_parameters(['foo', 'bar']) + self.assertIsInstance(descriptor_list, list) + self.assertEqual(len(descriptor_list), 2) + self.assertEqual(descriptor_list[0], ParameterDescriptor()) + self.assertEqual(descriptor_list[1], ParameterDescriptor()) + def test_node_get_parameter(self): self.node.set_parameters([Parameter('foo', Parameter.Type.INTEGER, 42)]) self.assertIsInstance(self.node.get_parameter('foo'), Parameter) @@ -355,17 +377,17 @@ def test_declare_parameter(self): 'reject_me', ParameterValue( type=Parameter.Type.STRING.value, string_value='raise'), ParameterDescriptor()) - with self.assertRaises(AssertionError): + with self.assertRaises(TypeError): self.node.declare_parameter( 1, ParameterValue(type=Parameter.Type.STRING.value, string_value='wrong_name_type'), ParameterDescriptor()) - with self.assertRaises(AssertionError): + with self.assertRaises(TypeError): self.node.declare_parameter( 'wrong_parameter_value_type', 1234, ParameterDescriptor()) - with self.assertRaises(AssertionError): + with self.assertRaises(TypeError): self.node.declare_parameter( 'wrong_parameter_descriptor_type', ParameterValue(), ParameterValue()) @@ -449,7 +471,7 @@ def test_declare_parameters(self): with self.assertRaises(InvalidParameterValueException): self.node.declare_parameters('', parameters) - with self.assertRaises(AssertionError): + with self.assertRaises(TypeError): self.node.declare_parameters( '', [( @@ -460,7 +482,7 @@ def test_declare_parameters(self): )] ) - with self.assertRaises(AssertionError): + with self.assertRaises(TypeError): self.node.declare_parameters( '', [( @@ -470,7 +492,7 @@ def test_declare_parameters(self): )] ) - with self.assertRaises(AssertionError): + with self.assertRaises(TypeError): self.node.declare_parameters( '', [( @@ -484,6 +506,38 @@ def reject_parameter_callback(self, parameter_list): rejected_parameters = (param for param in parameter_list if 'reject' in param.name) return SetParametersResult(successful=(not any(rejected_parameters))) + def test_node_undeclare_parameter_has_parameter(self): + # Undeclare unexisting parameter. + with self.assertRaises(ParameterNotDeclaredException): + self.node.undeclare_parameter('foo') + + # Verify that it doesn't exist. + self.assertFalse(self.node.has_parameter('foo')) + + # Declare parameter, verify existance, undeclare, and verify again. + self.node.declare_parameter( + 'foo', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor() + ) + self.assertTrue(self.node.has_parameter('foo')) + self.node.undeclare_parameter('foo') + self.assertFalse(self.node.has_parameter('foo')) + + # Try with a read only parameter. + self.assertFalse(self.node.has_parameter('immutable_foo')) + self.node.declare_parameter( + 'immutable_foo', + ParameterValue(type=Parameter.Type.STRING.value, string_value='I am immutable'), + ParameterDescriptor(read_only=True) + ) + with self.assertRaises(ParameterImmutableException): + self.node.undeclare_parameter('immutable_foo') + + # Verify that it still exists with the same value. + self.assertTrue(self.node.has_parameter('immutable_foo')) + self.assertEqual(self.node.get_parameter('immutable_foo').value, 'I am immutable') + def test_node_set_undeclared_parameters(self): with self.assertRaises(ParameterNotDeclaredException): self.node.set_parameters([ @@ -510,6 +564,528 @@ def test_node_get_undeclared_parameter_or(self): self.assertEqual(result.name, 'foo') self.assertEqual(result.value, 152) + def test_node_set_parameters(self): + parameter_tuples = [ + ( + 'foo', + ParameterValue(type=Parameter.Type.INTEGER.value, integer_value=42), + ParameterDescriptor() + ), + ( + 'bar', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor() + ), + ( + 'baz', + ParameterValue(type=Parameter.Type.DOUBLE.value, double_value=2.41), + ParameterDescriptor() + ) + ] + + # Create rclpy.Parameter list from tuples. + parameters = [ + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[0][0], value=parameter_tuples[0][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[1][0], value=parameter_tuples[1][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[2][0], value=parameter_tuples[2][1])), + ] + + with self.assertRaises(ParameterNotDeclaredException): + self.node.set_parameters(parameters) + + self.node.declare_parameters('', parameter_tuples) + result = self.node.set_parameters(parameters) + + # OK cases: check successful result and parameter value for each parameter set. + self.assertIsInstance(result, list) + self.assertIsInstance(result[0], SetParametersResult) + self.assertIsInstance(result[1], SetParametersResult) + self.assertIsInstance(result[2], SetParametersResult) + self.assertTrue(result[0].successful) + self.assertTrue(result[1].successful) + self.assertTrue(result[2].successful) + self.assertEqual(self.node.get_parameter('foo').value, 42) + self.assertEqual(self.node.get_parameter('bar').value, 'hello') + self.assertEqual(self.node.get_parameter('baz').value, 2.41) + + # Now we modify the declared parameters, add a new one and set them again. + parameter_tuples[0][1].integer_value = 24 + parameter_tuples[1][1].string_value = 'bye' + parameter_tuples[2][1].double_value = 1.42 + parameter_tuples.append( + ( + 'foobar', + ParameterValue(type=Parameter.Type.DOUBLE.value, double_value=2.71), + ParameterDescriptor()) + ) + parameters = [ + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[0][0], value=parameter_tuples[0][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[1][0], value=parameter_tuples[1][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[2][0], value=parameter_tuples[2][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[3][0], value=parameter_tuples[3][1])), + ] + # The first three parameters should have been set; the fourth one causes the exception. + with self.assertRaises(ParameterNotDeclaredException): + self.node.set_parameters(parameters) + + # Validate first three. + self.assertEqual(self.node.get_parameter('foo').value, 24) + self.assertEqual(self.node.get_parameter('bar').value, 'bye') + self.assertEqual(self.node.get_parameter('baz').value, 1.42) + + # Confirm that the fourth one does not exist. + with self.assertRaises(ParameterNotDeclaredException): + self.node.get_parameter('foobar') + + def test_node_set_parameters_rejection(self): + # Declare a new parameter and set a callback so that it's rejected when set. + reject_parameter_tuple = ( + 'reject_me', + ParameterValue(type=Parameter.Type.BOOL.value, bool_value=True), + ParameterDescriptor() + ) + + self.node.declare_parameter(*reject_parameter_tuple) + self.node.set_parameters_callback(self.reject_parameter_callback) + result = self.node.set_parameters( + [ + Parameter.from_parameter_msg( + ParameterMsg(name=reject_parameter_tuple[0], value=reject_parameter_tuple[1])) + ] + ) + self.assertIsInstance(result, list) + self.assertIsInstance(result[0], SetParametersResult) + self.assertFalse(result[0].successful) + + def test_node_set_parameters_read_only(self): + parameter_tuples = [ + ( + 'immutable_foo', + ParameterValue(type=Parameter.Type.INTEGER.value, integer_value=42), + ParameterDescriptor(read_only=True) + ), + ( + 'bar', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor() + ), + ( + 'immutable_baz', + ParameterValue(type=Parameter.Type.DOUBLE.value, double_value=2.41), + ParameterDescriptor(read_only=True) + ) + ] + + # Create rclpy.Parameter list from tuples. + parameters = [ + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[0][0], value=parameter_tuples[0][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[1][0], value=parameter_tuples[1][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[2][0], value=parameter_tuples[2][1])), + ] + + self.node.declare_parameters('', parameter_tuples) + + # Try setting a different value to the declared parameters. + parameter_tuples[0][1].integer_value = 24 + parameter_tuples[1][1].string_value = 'bye' + parameter_tuples[2][1].double_value = 1.42 + + # Re-create parameters from modified tuples. + parameters = [ + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[0][0], value=parameter_tuples[0][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[1][0], value=parameter_tuples[1][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[2][0], value=parameter_tuples[2][1])), + ] + + result = self.node.set_parameters(parameters) + + # Only the parameter that is not read_only should have succeeded. + self.assertIsInstance(result, list) + self.assertIsInstance(result[0], SetParametersResult) + self.assertIsInstance(result[1], SetParametersResult) + self.assertIsInstance(result[2], SetParametersResult) + self.assertFalse(result[0].successful) + self.assertTrue(result[1].successful) + self.assertFalse(result[2].successful) + self.assertEqual(self.node.get_parameter('immutable_foo').value, 42) + self.assertEqual(self.node.get_parameter('bar').value, 'bye') + self.assertEqual(self.node.get_parameter('immutable_baz').value, 2.41) + + def test_node_set_parameters_implicit_undeclare(self): + parameter_tuples = [ + ( + 'foo', + ParameterValue(type=Parameter.Type.INTEGER.value, integer_value=42), + ParameterDescriptor() + ), + ( + 'bar', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor() + ), + ( + 'baz', + ParameterValue(type=Parameter.Type.DOUBLE.value, double_value=2.41), + ParameterDescriptor() + ) + ] + + self.node.declare_parameters('', parameter_tuples) + + # Verify that the parameters are set. + self.assertEqual(self.node.get_parameter('foo').value, 42) + self.assertEqual(self.node.get_parameter('bar').value, 'hello') + self.assertEqual(self.node.get_parameter('baz').value, 2.41) + + # Now undeclare one of them implicitly. + self.node.set_parameters([Parameter('bar', Parameter.Type.NOT_SET, None)]) + 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) + + def test_node_set_parameters_atomically(self): + parameter_tuples = [ + ( + 'foo', + ParameterValue(type=Parameter.Type.INTEGER.value, integer_value=42), + ParameterDescriptor() + ), + ( + 'bar', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor() + ), + ( + 'baz', + ParameterValue(type=Parameter.Type.DOUBLE.value, double_value=2.41), + ParameterDescriptor() + ) + ] + + # Create rclpy.Parameter list from tuples. + parameters = [ + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[0][0], value=parameter_tuples[0][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[1][0], value=parameter_tuples[1][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[2][0], value=parameter_tuples[2][1])), + ] + + with self.assertRaises(ParameterNotDeclaredException): + self.node.set_parameters_atomically(parameters) + + self.node.declare_parameters('', parameter_tuples) + result = self.node.set_parameters_atomically(parameters) + + # OK case: check successful aggregated result. + self.assertIsInstance(result, SetParametersResult) + self.assertTrue(result.successful) + self.assertEqual(self.node.get_parameter('foo').value, 42) + self.assertEqual(self.node.get_parameter('bar').value, 'hello') + self.assertEqual(self.node.get_parameter('baz').value, 2.41) + + # Now we modify the declared parameters, add a new one and set them again. + parameter_tuples[0][1].integer_value = 24 + parameter_tuples[1][1].string_value = 'bye' + parameter_tuples[2][1].double_value = 1.42 + parameter_tuples.append( + ( + 'foobar', + ParameterValue(type=Parameter.Type.DOUBLE.value, double_value=2.71), + ParameterDescriptor()) + ) + parameters = [ + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[0][0], value=parameter_tuples[0][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[1][0], value=parameter_tuples[1][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[2][0], value=parameter_tuples[2][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[3][0], value=parameter_tuples[3][1])), + ] + + # The fourth parameter causes the exception, hence none is set. + with self.assertRaises(ParameterNotDeclaredException): + self.node.set_parameters_atomically(parameters) + + # Confirm that the first three were not modified. + self.assertEqual(self.node.get_parameter('foo').value, 42) + self.assertEqual(self.node.get_parameter('bar').value, 'hello') + self.assertEqual(self.node.get_parameter('baz').value, 2.41) + + # Confirm that the fourth one does not exist. + with self.assertRaises(ParameterNotDeclaredException): + self.node.get_parameter('foobar') + + def test_node_set_parameters_atomically_rejection(self): + # Declare a new parameter and set a callback so that it's rejected when set. + reject_parameter_tuple = ( + 'reject_me', + ParameterValue(type=Parameter.Type.BOOL.value, bool_value=True), + ParameterDescriptor() + ) + + self.node.declare_parameter(*reject_parameter_tuple) + self.node.set_parameters_callback(self.reject_parameter_callback) + result = self.node.set_parameters_atomically( + [ + Parameter.from_parameter_msg( + ParameterMsg(name=reject_parameter_tuple[0], value=reject_parameter_tuple[1])) + ] + ) + self.assertIsInstance(result, SetParametersResult) + self.assertFalse(result.successful) + + def test_node_set_parameters_atomically_read_only(self): + parameter_tuples = [ + ( + 'foo', + ParameterValue(type=Parameter.Type.INTEGER.value, integer_value=42), + ParameterDescriptor() + ), + ( + 'bar', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor() + ), + ( + 'immutable_baz', + ParameterValue(type=Parameter.Type.DOUBLE.value, double_value=2.41), + ParameterDescriptor(read_only=True) + ) + ] + + # Create rclpy.Parameter list from tuples. + parameters = [ + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[0][0], value=parameter_tuples[0][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[1][0], value=parameter_tuples[1][1])), + Parameter.from_parameter_msg( + ParameterMsg(name=parameter_tuples[2][0], value=parameter_tuples[2][1])), + ] + + self.node.declare_parameters('', parameter_tuples) + + # Try setting a different value to the declared parameters. + parameter_tuples[0][1].integer_value = 24 + parameter_tuples[1][1].string_value = 'bye' + parameter_tuples[2][1].double_value = 1.42 + + result = self.node.set_parameters_atomically(parameters) + + # At least one parameter is read-only, so the overall result should be a failure. + # All the parameters should have their original value. + self.assertIsInstance(result, SetParametersResult) + self.assertFalse(result.successful) + self.assertEqual(self.node.get_parameter('foo').value, 42) + self.assertEqual(self.node.get_parameter('bar').value, 'hello') + self.assertEqual(self.node.get_parameter('immutable_baz').value, 2.41) + + def test_node_set_parameters_atomically_implicit_undeclare(self): + parameter_tuples = [ + ( + 'foo', + ParameterValue(type=Parameter.Type.INTEGER.value, integer_value=42), + ParameterDescriptor() + ), + ( + 'bar', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor() + ), + ( + 'baz', + ParameterValue(type=Parameter.Type.DOUBLE.value, double_value=2.41), + ParameterDescriptor() + ) + ] + + self.node.declare_parameters('', parameter_tuples) + + # Verify that the parameters are set. + self.assertEqual(self.node.get_parameter('foo').value, 42) + self.assertEqual(self.node.get_parameter('bar').value, 'hello') + 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)]) + 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) + + def test_describe_parameter(self): + with self.assertRaises(ParameterNotDeclaredException): + self.node.describe_parameter('foo') + + # Declare parameter with descriptor. + self.node.declare_parameter( + 'foo', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor( + name='foo', + type=ParameterType.PARAMETER_STRING, + additional_constraints='some constraints', + read_only=True, + floating_point_range=[FloatingPointRange(from_value=-2.0, to_value=2.0, step=0.1)], + integer_range=[IntegerRange(from_value=-10, to_value=10, step=2)] + ) + ) + + descriptor = self.node.describe_parameter('foo') + self.assertEqual(descriptor.name, 'foo') + self.assertEqual(descriptor.type, ParameterType.PARAMETER_STRING) + self.assertEqual(descriptor.additional_constraints, 'some constraints') + self.assertEqual(descriptor.read_only, True) + self.assertEqual(descriptor.floating_point_range[0].from_value, -2.0) + self.assertEqual(descriptor.floating_point_range[0].to_value, 2.0) + self.assertEqual(descriptor.floating_point_range[0].step, 0.1) + self.assertEqual(descriptor.integer_range[0].from_value, -10) + self.assertEqual(descriptor.integer_range[0].to_value, 10) + self.assertEqual(descriptor.integer_range[0].step, 2) + + def test_describe_parameters(self): + with self.assertRaises(ParameterNotDeclaredException): + self.node.describe_parameter('foo') + with self.assertRaises(ParameterNotDeclaredException): + self.node.describe_parameter('bar') + + # Declare parameters with descriptors. + self.node.declare_parameter( + 'foo', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor( + name='foo', + type=ParameterType.PARAMETER_STRING, + additional_constraints='some constraints', + read_only=True, + floating_point_range=[FloatingPointRange(from_value=-2.0, to_value=2.0, step=0.1)], + integer_range=[IntegerRange(from_value=-10, to_value=10, step=2)] + ) + ) + self.node.declare_parameter( + 'bar', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor( + name='bar', + type=ParameterType.PARAMETER_INTEGER, + additional_constraints='some more constraints', + read_only=True, + floating_point_range=[FloatingPointRange(from_value=-3.0, to_value=3.0, step=0.3)], + integer_range=[IntegerRange(from_value=-20, to_value=20, step=3)] + ) + ) + + # Check list. + descriptor_list = self.node.describe_parameters(['foo', 'bar']) + self.assertIsInstance(descriptor_list, list) + self.assertEqual(len(descriptor_list), 2) + + # Check individual descriptors. + foo_descriptor = descriptor_list[0] + self.assertEqual(foo_descriptor.name, 'foo') + self.assertEqual(foo_descriptor.type, ParameterType.PARAMETER_STRING) + self.assertEqual(foo_descriptor.additional_constraints, 'some constraints') + self.assertEqual(foo_descriptor.read_only, True) + self.assertEqual(foo_descriptor.floating_point_range[0].from_value, -2.0) + self.assertEqual(foo_descriptor.floating_point_range[0].to_value, 2.0) + self.assertEqual(foo_descriptor.floating_point_range[0].step, 0.1) + self.assertEqual(foo_descriptor.integer_range[0].from_value, -10) + self.assertEqual(foo_descriptor.integer_range[0].to_value, 10) + self.assertEqual(foo_descriptor.integer_range[0].step, 2) + + bar_descriptor = descriptor_list[1] + self.assertEqual(bar_descriptor.name, 'bar') + self.assertEqual(bar_descriptor.type, ParameterType.PARAMETER_INTEGER) + self.assertEqual(bar_descriptor.additional_constraints, 'some more constraints') + self.assertEqual(bar_descriptor.read_only, True) + self.assertEqual(bar_descriptor.floating_point_range[0].from_value, -3.0) + self.assertEqual(bar_descriptor.floating_point_range[0].to_value, 3.0) + self.assertEqual(bar_descriptor.floating_point_range[0].step, 0.3) + self.assertEqual(bar_descriptor.integer_range[0].from_value, -20) + self.assertEqual(bar_descriptor.integer_range[0].to_value, 20) + self.assertEqual(bar_descriptor.integer_range[0].step, 3) + + # TODO(jubeira): test cases failing because of non-compliant existing parameter values + # once the feature is implemented. + def test_set_descriptor(self): + with self.assertRaises(ParameterNotDeclaredException): + self.node.set_descriptor('foo', ParameterDescriptor()) + + # Declare parameter with default descriptor. + self.node.declare_parameter( + 'foo', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor() + ) + self.assertEqual(self.node.describe_parameter('foo'), ParameterDescriptor()) + + # Now modify the descriptor and check again. + value = self.node.set_descriptor( + 'foo', + ParameterDescriptor( + name='foo', + type=ParameterType.PARAMETER_STRING, + additional_constraints='some constraints', + read_only=True, + floating_point_range=[FloatingPointRange(from_value=-2.0, to_value=2.0, step=0.1)], + integer_range=[IntegerRange(from_value=-10, to_value=10, step=2)] + ) + ) + self.assertEqual(value.type, Parameter.Type.STRING.value) + self.assertEqual(value.string_value, 'hello') + + descriptor = self.node.describe_parameter('foo') + self.assertEqual(descriptor.name, 'foo') + self.assertEqual(descriptor.type, ParameterType.PARAMETER_STRING) + self.assertEqual(descriptor.additional_constraints, 'some constraints') + self.assertEqual(descriptor.read_only, True) + self.assertEqual(descriptor.floating_point_range[0].from_value, -2.0) + self.assertEqual(descriptor.floating_point_range[0].to_value, 2.0) + self.assertEqual(descriptor.floating_point_range[0].step, 0.1) + self.assertEqual(descriptor.integer_range[0].from_value, -10) + self.assertEqual(descriptor.integer_range[0].to_value, 10) + self.assertEqual(descriptor.integer_range[0].step, 2) + + def test_set_descriptor_read_only(self): + with self.assertRaises(ParameterNotDeclaredException): + self.node.set_descriptor('foo', ParameterDescriptor()) + + # Declare parameter with a read_only descriptor. + self.node.declare_parameter( + 'foo', + ParameterValue(type=Parameter.Type.STRING.value, string_value='hello'), + ParameterDescriptor(read_only=True) + ) + self.assertEqual(self.node.describe_parameter('foo'), ParameterDescriptor(read_only=True)) + + # Try modifying the descriptor. + with self.assertRaises(ParameterImmutableException): + self.node.set_descriptor( + 'foo', + ParameterDescriptor( + name='foo', + type=ParameterType.PARAMETER_STRING, + additional_constraints='some constraints', + read_only=False, + ) + ) + class TestCreateNode(unittest.TestCase):