Implement node parameters and parameter services

rclpy/rclpy/__init__.py

@@ -49,7 +49,10 @@ def shutdown():
     _shutdown()
 
 
-def create_node(node_name, *, cli_args=None, namespace=None, use_global_arguments=True):
+def create_node(
+    node_name, *, cli_args=None, namespace=None, use_global_arguments=True,
+    start_parameter_services=True
+):
     """
     Create an instance of :class:`rclpy.node.Node`.
 
@@ -64,7 +67,8 @@ def create_node(node_name, *, cli_args=None, namespace=None, use_global_argument
     from rclpy.node import Node
     return Node(
         node_name, cli_args=cli_args, namespace=namespace,
-        use_global_arguments=use_global_arguments)
+        use_global_arguments=use_global_arguments,
+        start_parameter_services=start_parameter_services)
 
 
 def spin_once(node, *, timeout_sec=None):

rclpy/rclpy/node.py

@@ -14,6 +14,7 @@
 
 import weakref
 
+from rcl_interfaces.msg import SetParametersResult
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.client import Client
 from rclpy.clock import ROSClock
@@ -24,6 +25,8 @@
 from rclpy.guard_condition import GuardCondition
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 from rclpy.logging import get_logger
+from rclpy.parameter import Parameter
+from rclpy.parameter_service import ParameterService
 from rclpy.publisher import Publisher
 from rclpy.qos import qos_profile_default, qos_profile_services_default
 from rclpy.service import Service
@@ -55,15 +58,20 @@ def check_for_type_support(msg_type):
 
 class Node:
 
-    def __init__(self, node_name, *, cli_args=None, namespace=None, use_global_arguments=True):
+    def __init__(
+        self, node_name, *, cli_args=None, namespace=None, use_global_arguments=True,
+        start_parameter_services=True
+    ):
         self._handle = None
+        self._parameters = {}
         self.publishers = []
         self.subscriptions = []
         self.clients = []
         self.services = []
         self.timers = []
         self.guards = []
         self._default_callback_group = MutuallyExclusiveCallbackGroup()
+        self._parameters_callback = None
 
         namespace = namespace or ''
         if not ok():
@@ -92,6 +100,9 @@ def __init__(self, node_name, *, cli_args=None, namespace=None, use_global_argum
 
         self.__executor_weakref = None
 
+        if start_parameter_services:
+            self._parameter_service = ParameterService(self)
+
     @property
     def executor(self):
         """Get the executor if the node has been added to one, else return None."""
@@ -129,6 +140,36 @@ def get_clock(self):
     def get_logger(self):
         return self._logger
 
+    def get_parameters(self, names):
+        if not all(isinstance(name, str) for name in names):
+            raise TypeError('All names must be instances of type str')
+        return [self.get_parameter(name) for name in names]
+
+    def get_parameter(self, name):
+        if name not in self._parameters:
+            return Parameter(name, Parameter.Type.NOT_SET, None)
+        return self._parameters[name]
+
+    def set_parameters(self, parameter_list):
+        results = []
+        for param in parameter_list:
+            if not isinstance(param, Parameter):
+                raise TypeError("parameter must be instance of type '{}'".format(repr(Parameter)))
+            results.append(self.set_parameters_atomically([param]))
+        return results
+
+    def set_parameters_atomically(self, parameter_list):
+        result = None
+        if self._parameters_callback:
+            result = self._parameters_callback(parameter_list)
+        else:
+            result = SetParametersResult(successful=True)
+
+        if result.successful:
+            for param in parameter_list:
+                self._parameters[param.name] = param
+        return result
+
     def _validate_topic_or_service_name(self, topic_or_service_name, *, is_service=False):
         name = self.get_name()
         namespace = self.get_namespace()

rclpy/rclpy/parameter.py

@@ -0,0 +1,134 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from enum import Enum
+
+from rcl_interfaces.msg import ParameterDescriptor, ParameterType, ParameterValue
+
+PARAMETER_SEPARATOR_STRING = '.'
+
+
+class Parameter:
+
+    class Type(Enum):
+        NOT_SET = ParameterType.PARAMETER_NOT_SET
+        BOOL = ParameterType.PARAMETER_BOOL
+        INTEGER = ParameterType.PARAMETER_INTEGER
+        DOUBLE = ParameterType.PARAMETER_DOUBLE
+        STRING = ParameterType.PARAMETER_STRING
+        BYTE_ARRAY = ParameterType.PARAMETER_BYTE_ARRAY
+        BOOL_ARRAY = ParameterType.PARAMETER_BOOL_ARRAY
+        INTEGER_ARRAY = ParameterType.PARAMETER_INTEGER_ARRAY
+        DOUBLE_ARRAY = ParameterType.PARAMETER_DOUBLE_ARRAY
+        STRING_ARRAY = ParameterType.PARAMETER_STRING_ARRAY
+
+        def check(self, parameter_value):
+            if Parameter.Type.NOT_SET == self:
+                return parameter_value is None
+            if Parameter.Type.BOOL == self:
+                return isinstance(parameter_value, bool)
+            if Parameter.Type.INTEGER == self:
+                return isinstance(parameter_value, int)
+            if Parameter.Type.DOUBLE == self:
+                return isinstance(parameter_value, float)
+            if Parameter.Type.STRING == self:
+                return isinstance(parameter_value, str)
+            if Parameter.Type.BYTE_ARRAY == self:
+                return isinstance(parameter_value, list) and \
+                    all(isinstance(v, bytes) and len(v) == 1 for v in parameter_value)
+            if Parameter.Type.BOOL_ARRAY == self:
+                return isinstance(parameter_value, list) and \
+                    all(isinstance(v, bool) for v in parameter_value)
+            if Parameter.Type.INTEGER_ARRAY == self:
+                return isinstance(parameter_value, list) and \
+                    all(isinstance(v, int) for v in parameter_value)
+            if Parameter.Type.DOUBLE_ARRAY == self:
+                return isinstance(parameter_value, list) and \
+                    all(isinstance(v, float) for v in parameter_value)
+            if Parameter.Type.STRING_ARRAY == self:
+                return isinstance(parameter_value, list) and \
+                    all(isinstance(v, str) for v in parameter_value)
+            return False
+
+    @classmethod
+    def from_rcl_interface_parameter(cls, rcl_param):
+        value = None
+        type_ = Parameter.Type(value=rcl_param.value.type)
+        if Parameter.Type.BOOL == type_:
+            value = rcl_param.value.bool_value
+        elif Parameter.Type.INTEGER == type_:
+            value = rcl_param.value.integer_value
+        elif Parameter.Type.DOUBLE == type_:
+            value = rcl_param.value.double_value
+        elif Parameter.Type.STRING == type_:
+            value = rcl_param.value.string_value
+        elif Parameter.Type.BYTE_ARRAY == type_:
+            value = rcl_param.value.byte_array_value
+        elif Parameter.Type.BOOL_ARRAY == type_:
+            value = rcl_param.value.bool_array_value
+        elif Parameter.Type.INTEGER_ARRAY == type_:
+            value = rcl_param.value.integer_array_value
+        elif Parameter.Type.DOUBLE_ARRAY == type_:
+            value = rcl_param.value.double_array_value
+        elif Parameter.Type.STRING_ARRAY == type_:
+            value = rcl_param.value.string_array_value
+        return cls(rcl_param.name, type_, value)
+
+    def __init__(self, name, type_, value=None):
+        if not isinstance(type_, Parameter.Type):
+            raise TypeError("type must be an instance of '{}'".format(repr(Parameter.Type)))
+
+        if not type_.check(value):
+            raise ValueError("Type '{}' and value '{}' do not agree".format(type_, value))
+
+        self._type_ = type_
+        self._name = name
+        self._value = value
+
+    @property
+    def name(self):
+        return self._name
+
+    @property
+    def type_(self):
+        return self._type_
+
+    @property
+    def value(self):
+        return self._value
+
+    def get_descriptor(self):
+        return ParameterDescriptor(name=self.name, type=self.type_.value)
+
+    def get_parameter_value(self):
+        parameter_value = ParameterValue(type=self.type_.value)
+        if Parameter.Type.BOOL == self.type_:
+            parameter_value.bool_value = self.value
+        elif Parameter.Type.INTEGER == self.type_:
+            parameter_value.integer_value = self.value
+        elif Parameter.Type.DOUBLE == self.type_:
+            parameter_value.double_value = self.value
+        elif Parameter.Type.STRING == self.type_:
+            parameter_value.string_value = self.value
+        elif Parameter.Type.BYTE_ARRAY == self.type_:
+            parameter_value.byte_array_value = self.value
+        elif Parameter.Type.BOOL_ARRAY == self.type_:
+            parameter_value.bool_array_value = self.value
+        elif Parameter.Type.INTEGER_ARRAY == self.type_:
+            parameter_value.integer_array_value = self.value
+        elif Parameter.Type.DOUBLE_ARRAY == self.type_:
+            parameter_value.double_array_value = self.value
+        elif Parameter.Type.STRING_ARRAY == self.type_:
+            parameter_value.string_array_value = self.value
+        return parameter_value

rclpy/rclpy/parameter_service.py

@@ -0,0 +1,127 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from rcl_interfaces.srv import DescribeParameters, GetParameters, GetParameterTypes
+from rcl_interfaces.srv import ListParameters, SetParameters, SetParametersAtomically
+from rclpy.parameter import Parameter, PARAMETER_SEPARATOR_STRING
+from rclpy.qos import qos_profile_parameters
+from rclpy.validate_topic_name import TOPIC_SEPARATOR_STRING
+
+
+class ParameterService:
+
+    def __init__(self, node):
+        self._node = node
+        nodename = node.get_name()
+
+        describe_parameters_service_name = \
+            TOPIC_SEPARATOR_STRING.join((nodename, 'describe_parameters'))
+        node.create_service(
+            DescribeParameters, describe_parameters_service_name,
+            self._describe_parameters_callback, qos_profile=qos_profile_parameters
+        )
+        get_parameters_service_name = TOPIC_SEPARATOR_STRING.join((nodename, 'get_parameters'))
+        node.create_service(
+            GetParameters, get_parameters_service_name, self._get_parameters_callback,
+            qos_profile=qos_profile_parameters
+        )
+        get_parameter_types_service_name = \
+            TOPIC_SEPARATOR_STRING.join((nodename, 'get_parameter_types'))
+        node.create_service(
+            GetParameterTypes, get_parameter_types_service_name,
+            self._get_parameter_types_callback, qos_profile=qos_profile_parameters
+        )
+        list_parameters_service_name = TOPIC_SEPARATOR_STRING.join((nodename, 'list_parameters'))
+        node.create_service(
+            ListParameters, list_parameters_service_name, self._list_parameters_callback,
+            qos_profile=qos_profile_parameters
+        )
+        set_parameters_service_name = TOPIC_SEPARATOR_STRING.join((nodename, 'set_parameters'))
+        node.create_service(
+            SetParameters, set_parameters_service_name, self._set_parameters_callback,
+            qos_profile=qos_profile_parameters
+        )
+        set_parameters_atomically_service_name = \
+            TOPIC_SEPARATOR_STRING.join((nodename, 'set_parameters_atomically'))
+        node.create_service(
+            SetParametersAtomically, set_parameters_atomically_service_name,
+            self._set_parameters_atomically_callback,
+            qos_profile=qos_profile_parameters
+        )
+
+    def _describe_parameters_callback(self, request, response):
+        for name in request.names:
+            p = self._node.get_parameter(name)
+            response.descriptors.append(p.get_descriptor())
+        return response
+
+    def _get_parameters_callback(self, request, response):
+        for name in request.names:
+            p = self._node.get_parameter(name)
+            response.values.append(p.get_parameter_value())
+        return response
+
+    def _get_parameter_types_callback(self, request, response):
+        for name in request.names:
+            response.types.append(self._node.get_parameter(name).get_parameter_type())
+        return response
+
+    def _list_parameters_callback(self, request, response):
+        names_with_prefixes = []
+        for name in self._node._parameters.keys():
+            if PARAMETER_SEPARATOR_STRING in name:
+                names_with_prefixes.append(name)
+                continue
+            elif request.prefixes:
+                continue
+            else:
+                response.result.names.append(name)
+        if 1 == request.depth:
+            return response
+
+        if not request.DEPTH_RECURSIVE == request.depth:
+            names_with_prefixes = filter(
+                lambda name:
+                    name.count(PARAMETER_SEPARATOR_STRING) < request.depth, names_with_prefixes
+            )
+        for name in names_with_prefixes:
+            if request.prefixes:
+                for prefix in request.prefixes:
+                    if name.startswith(prefix + PARAMETER_SEPARATOR_STRING):
+                        response.result.names.append(name)
+                        full_prefix = PARAMETER_SEPARATOR_STRING.join(
+                            name.split(PARAMETER_SEPARATOR_STRING)[0:-1])
+                        if full_prefix not in response.result.prefixes:
+                            response.result.prefixes.append(full_prefix)
+                        if prefix not in response.result.prefixes:
+                            response.result.prefixes.append(prefix)
+            else:
+                prefix = PARAMETER_SEPARATOR_STRING.join(
+                    name.split(PARAMETER_SEPARATOR_STRING)[0:-1])
+                if prefix not in response.result.prefixes:
+                    response.result.prefixes.append(prefix)
+                response.result.names.append(name)
+
+        return response
+
+    def _set_parameters_callback(self, request, response):
+        for p in request.parameters:
+            param = Parameter.from_rcl_interface_parameter(p)
+            response.results.append(self._node.set_parameters_atomically([param]))
+        return response
+
+    def _set_parameters_atomically_callback(self, request, response):
+        response.results = self._node.set_parameters_atomically([
+            Parameter.from_rcl_interface_parameter(p) for p in request.parameters])
+        return response

rclpy/rclpy/validate_topic_name.py

@@ -16,6 +16,8 @@
 from rclpy.exceptions import InvalidTopicNameException
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 
+TOPIC_SEPARATOR_STRING = '/'
+
 
 def validate_topic_name(name, *, is_service=False):
     """