implement parameter_client (#959)

Signed-off-by: Brian Chen <[email protected]>

Co-authored-by: Tomoya Fujita <[email protected]>
Co-authored-by: Jacob Perron <[email protected]>

Author: 3 people

rclpy/docs/source/api/parameters.rst

@@ -10,3 +10,8 @@ Parameter Service
 -----------------
 
 .. automodule:: rclpy.parameter_service
+
+Parameter Client
+-----------------
+
+.. automodule:: rclpy.parameter_client

rclpy/rclpy/client.py

@@ -170,6 +170,7 @@ def wait_for_service(self, timeout_sec: float = None) -> bool:
         """
         # TODO(sloretz) Return as soon as the service is available
         # This is a temporary implementation. The sleep time is arbitrary.
+        # https://github.com/ros2/rclpy/issues/58
         sleep_time = 0.25
         if timeout_sec is None:
             timeout_sec = float('inf')

rclpy/rclpy/parameter.py

@@ -14,9 +14,14 @@
 
 import array
 from enum import Enum
+from typing import Dict
+from typing import List
+from typing import Optional
 
 from rcl_interfaces.msg import Parameter as ParameterMsg
-from rcl_interfaces.msg import ParameterType, ParameterValue
+from rcl_interfaces.msg import ParameterType
+from rcl_interfaces.msg import ParameterValue
+import yaml
 
 PARAMETER_SEPARATOR_STRING = '.'
 
@@ -177,6 +182,50 @@ def to_parameter_msg(self):
         return ParameterMsg(name=self.name, value=self.get_parameter_value())
 
 
+def get_parameter_value(string_value: str) -> ParameterValue:
+    """
+    Guess the desired type of the parameter based on the string value.
+
+    :param string_value: The string value to be converted to a ParameterValue.
+    :return: The ParameterValue.
+    """
+    value = ParameterValue()
+    try:
+        yaml_value = yaml.safe_load(string_value)
+    except yaml.parser.ParserError:
+        yaml_value = string_value
+
+    if isinstance(yaml_value, bool):
+        value.type = ParameterType.PARAMETER_BOOL
+        value.bool_value = yaml_value
+    elif isinstance(yaml_value, int):
+        value.type = ParameterType.PARAMETER_INTEGER
+        value.integer_value = yaml_value
+    elif isinstance(yaml_value, float):
+        value.type = ParameterType.PARAMETER_DOUBLE
+        value.double_value = yaml_value
+    elif isinstance(yaml_value, list):
+        if all((isinstance(v, bool) for v in yaml_value)):
+            value.type = ParameterType.PARAMETER_BOOL_ARRAY
+            value.bool_array_value = yaml_value
+        elif all((isinstance(v, int) for v in yaml_value)):
+            value.type = ParameterType.PARAMETER_INTEGER_ARRAY
+            value.integer_array_value = yaml_value
+        elif all((isinstance(v, float) for v in yaml_value)):
+            value.type = ParameterType.PARAMETER_DOUBLE_ARRAY
+            value.double_array_value = yaml_value
+        elif all((isinstance(v, str) for v in yaml_value)):
+            value.type = ParameterType.PARAMETER_STRING_ARRAY
+            value.string_array_value = yaml_value
+        else:
+            value.type = ParameterType.PARAMETER_STRING
+            value.string_value = string_value
+    else:
+        value.type = ParameterType.PARAMETER_STRING
+        value.string_value = yaml_value
+    return value
+
+
 def parameter_value_to_python(parameter_value: ParameterValue):
     """
     Get the value for the Python builtin type from a rcl_interfaces/msg/ParameterValue object.
@@ -211,3 +260,79 @@ def parameter_value_to_python(parameter_value: ParameterValue):
         raise RuntimeError(f'unexpected parameter type {parameter_value.type}')
 
     return value
+
+
+def parameter_dict_from_yaml_file(
+    parameter_file: str,
+    use_wildcard: bool = False,
+    target_nodes: Optional[List[str]] = None,
+    namespace: str = ''
+) -> Dict[str, ParameterMsg]:
+    """
+    Build a dict of parameters from a YAML file.
+
+    Will load all parameters if ``target_nodes`` is None or empty.
+
+    :raises RuntimeError: if a target node is not in the file
+    :raises RuntimeError: if the is not a valid ROS parameter file
+
+    :param parameter_file: Path to the YAML file to load parameters from.
+    :param use_wildcard: Use wildcard matching for the target nodes.
+    :param target_nodes: List of nodes in the YAML file to load parameters from.
+    :param namespace: Namespace to prepend to all parameters.
+    :return: A dict of Parameter messages keyed by the parameter names
+    """
+    with open(parameter_file, 'r') as f:
+        param_file = yaml.safe_load(f)
+        param_keys = []
+        param_dict = {}
+
+        if use_wildcard and '/**' in param_file:
+            param_keys.append('/**')
+
+        if target_nodes:
+            for n in target_nodes:
+                if n not in param_file.keys():
+                    raise RuntimeError(f'Param file does not contain parameters for {n},'
+                                       f'only for nodes: {list(param_file.keys())} ')
+                param_keys.append(n)
+        else:
+            # wildcard key must go to the front of param_keys so that
+            # node-namespaced parameters will override the wildcard parameters
+            keys = set(param_file.keys())
+            keys.discard('/**')
+            param_keys.extend(keys)
+
+        if len(param_keys) == 0:
+            raise RuntimeError('Param file does not contain selected parameters')
+
+        for n in param_keys:
+            value = param_file[n]
+            if type(value) != dict or 'ros__parameters' not in value:
+                raise RuntimeError(f'YAML file is not a valid ROS parameter file for node {n}')
+            param_dict.update(value['ros__parameters'])
+        return _unpack_parameter_dict(namespace, param_dict)
+
+
+def _unpack_parameter_dict(namespace, parameter_dict):
+    """
+    Flatten a parameter dictionary recursively.
+
+    :param namespace: The namespace to prepend to the parameter names.
+    :param parameter_dict: A dictionary of parameters keyed by the parameter names
+    :return: A dict of Parameter objects keyed by the parameter names
+    """
+    parameters: Dict[str, ParameterMsg] = {}
+    for param_name, param_value in parameter_dict.items():
+        full_param_name = namespace + param_name
+        # Unroll nested parameters
+        if type(param_value) == dict:
+            parameters.update(_unpack_parameter_dict(
+                    namespace=full_param_name + PARAMETER_SEPARATOR_STRING,
+                    parameter_dict=param_value))
+        else:
+            parameter = ParameterMsg()
+            parameter.name = full_param_name
+            parameter.value = get_parameter_value(str(param_value))
+            parameters[full_param_name] = parameter
+    return parameters