From a39ea62cb69685e437a5935cfc206a648808d26a Mon Sep 17 00:00:00 2001 From: Juan Ignacio Ubeira Date: Thu, 2 May 2019 11:41:12 -0300 Subject: [PATCH] Changing signature for declare_parameters to support namespaces. Signed-off-by: Juan Ignacio Ubeira --- rclpy/rclpy/node.py | 54 +++++++++++++++++++++++++++++----------- rclpy/rclpy/parameter.py | 4 +-- 2 files changed, 41 insertions(+), 17 deletions(-) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index b5758e9ff..77859d3e9 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -20,8 +20,10 @@ import weakref +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 ParameterValue from rcl_interfaces.msg import SetParametersResult from rclpy.callback_groups import CallbackGroup from rclpy.callback_groups import MutuallyExclusiveCallbackGroup @@ -229,22 +231,33 @@ def get_logger(self): """Get the nodes logger.""" return self._logger - def declare_parameter(self, parameter: Parameter) -> Parameter: + def declare_parameter( + self, + name: str, + value: ParameterValue, + descriptor: ParameterDescriptor + ) -> Parameter: """ Declare and initialize a parameter. This method, if successful, will result in any callback registered with :func:`set_parameters_callback` to be called. - :param parameter: The parameter to declare, with a name, value and descriptor. + :param name: Name of the parameter to declare. + :param value: Value of the parameter to declare. + :param descriptor: Descriptor for the parameter to declare. :return: Parameter with the effectively assigned value. :raises: ParameterAlreadyDeclaredException if the parameter had already been declared. :raises: InvalidParameterException if the parameter name is invalid. :raises: InvalidParameterValueException if the registered callback rejects the parameter. """ - return self.declare_parameters('/', [parameter]) + return self.declare_parameters('', [(name, value, descriptor)])[0] - def declare_parameters(self, namespace: str, parameters: List[Parameter]) -> List[Parameter]: + def declare_parameters( + self, + namespace: str, + parameters: List[Tuple[str, ParameterValue, ParameterDescriptor]] + ) -> List[Parameter]: """ Declare a list of parameters. @@ -254,27 +267,38 @@ def declare_parameters(self, namespace: str, parameters: List[Parameter]) -> Lis not be declared. Parameters declared up to that point will not be undeclared. :param namespace: Namespace for parameters. - :param parameters: The parameters to declare, with a name, value and descriptor. + :param parameters: Tuple with parameters to declare, with a name, value and descriptor. :return: Parameter list with the effectively assigned values for each of them. :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. """ - for parameter in parameters: - parameter.name = namespace + parameter.name + parameter_list = [] + for parameter_tuple in parameters: + name = parameter_tuple[0] + assert isinstance(name, str) + value = parameter_tuple[1] + assert isinstance(value, ParameterValue) + descriptor = parameter_tuple[2] + assert isinstance(descriptor, ParameterDescriptor) + # Note(jubeira): declare_parameters verifies the name, but set_parameters doesn't. - validate_parameter_name(parameter.name) + full_name = namespace + name + validate_parameter_name(full_name) + + parameter_list.append(Parameter.from_parameter_msg( + ParameterMsg(name=full_name, value=value), descriptor)) parameters_already_declared = ( - parameter.name in self._parameters for parameter in parameters + parameter.name in self._parameters for parameter in parameter_list ) if any(parameters_already_declared): raise ParameterAlreadyDeclaredException(list(parameters_already_declared)) # Call the callback once for each of the parameters, using method that doesn't # check whether the parameter was declared beforehand or not. - self._set_parameters(parameters, raise_on_failure=True) - return self.get_parameters([parameter.name for parameter in parameters]) + self._set_parameters(parameter_list, raise_on_failure=True) + return self.get_parameters([parameter.name for parameter in parameter_list]) def undeclare_parameter(self, name: str): """ @@ -380,10 +404,10 @@ def set_parameters(self, parameter_list: List[Parameter]) -> List[SetParametersR return self._set_parameters(parameter_list, self.set_parameters_atomically) def _set_parameters( - self, - parameter_list: List[Parameter], - setter_func=None, - raise_on_failure=False + self, + parameter_list: List[Parameter], + setter_func=None, + raise_on_failure=False ) -> List[SetParametersResult]: """ Set parameters for the node, and return the result for the set action. diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 35c4a1709..492d37f59 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -63,7 +63,7 @@ def check(self, parameter_value): return False @classmethod - def from_parameter_msg(cls, param_msg): + def from_parameter_msg(cls, param_msg, descriptor=ParameterDescriptor()): value = None type_ = Parameter.Type(value=param_msg.value.type) if Parameter.Type.BOOL == type_: @@ -84,7 +84,7 @@ def from_parameter_msg(cls, param_msg): value = param_msg.value.double_array_value elif Parameter.Type.STRING_ARRAY == type_: value = param_msg.value.string_array_value - return cls(param_msg.name, type_, value) + return cls(param_msg.name, type_, value, descriptor) def __init__(self, name, type_, value=None, descriptor=ParameterDescriptor()): """