Merge branch 'ros2:rolling' into hliberacki/spin_until

.github/workflows/mirror-rolling-to-master.yaml

@@ -0,0 +1,13 @@
+name: Mirror rolling to master
+
+on:
+  push:
+    branches: [ rolling ]
+
+jobs:
+  mirror-to-master:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: zofrex/mirror-branch@v1
+      with:
+        target-branch: master

rclpy/docs/source/api/parameters.rst

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

rclpy/docs/source/conf.py

@@ -58,6 +58,7 @@
     'sphinx.ext.autosummary',
     'sphinx.ext.doctest',
     'sphinx.ext.coverage',
+    'sphinx_rtd_theme',
 ]
 
 # Add any paths that contain templates here, relative to this directory.
@@ -93,7 +94,7 @@
 # The theme to use for HTML and HTML Help pages.  See the documentation for
 # a list of builtin themes.
 #
-html_theme = 'alabaster'
+html_theme = 'sphinx_rtd_theme'
 
 # Theme options are theme-specific and customize the look and feel of a theme
 # further.  For a list of options available for each theme, see the

rclpy/package.xml

@@ -48,6 +48,7 @@
   <test_depend>test_msgs</test_depend>
 
   <doc_depend>python3-sphinx</doc_depend>
+  <doc_depend>python3-sphinx-rtd-theme</doc_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

rclpy/rclpy/client.py

@@ -101,7 +101,7 @@ def unblock(future):
 
     def call_async(self, request: SrvTypeRequest) -> Future:
         """
-        Make a service request and asyncronously get the result.
+        Make a service request and asynchronously get the result.
 
         :param request: The service request.
         :return: A future that completes when the request does.
@@ -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/executors.py

@@ -79,15 +79,15 @@ def __exit__(self, t, v, tb):
             self._num_work_executing -= 1
             self._work_condition.notify_all()
 
-    def wait(self, timeout_sec=None):
+    def wait(self, timeout_sec: Optional[float] = None):
         """
         Wait until all work completes.
 
         :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
         :type timeout_sec: float or None
         :rtype: bool True if all work completed
         """
-        if timeout_sec is not None and timeout_sec < 0:
+        if timeout_sec is not None and timeout_sec < 0.0:
             timeout_sec = None
         # Wait for all work to complete
         with self._work_condition:
@@ -204,9 +204,9 @@ def shutdown(self, timeout_sec: float = None) -> bool:
                 self._is_shutdown = True
                 # Tell executor it's been shut down
                 self._guard.trigger()
-
-        if not self._work_tracker.wait(timeout_sec):
-            return False
+        if not self._is_shutdown:
+            if not self._work_tracker.wait(timeout_sec):
+                return False
 
         # Clean up stuff that won't be used anymore
         with self._nodes_lock:
@@ -464,7 +464,13 @@ async def handler(entity, gc, is_shutdown, work_tracker):
                     entity.callback_group.ending_execution(entity)
                     # Signal that work has been done so the next callback in a mutually exclusive
                     # callback group can get executed
-                    gc.trigger()
+
+                    # Catch expected error where calling executor.shutdown()
+                    # from callback causes the GuardCondition to be destroyed
+                    try:
+                        gc.trigger()
+                    except InvalidHandle:
+                        pass
         task = Task(
             handler, (entity, self._guard, self._is_shutdown, self._work_tracker),
             executor=self)

rclpy/rclpy/node.py

@@ -349,6 +349,9 @@ def declare_parameter(
         This method, if successful, will result in any callback registered with
         :func:`add_on_set_parameters_callback` to be called.
 
+        The name and type in the given descriptor is ignored, and should be specified using
+        the name argument to this function and the default value's type instead.
+
         :param name: Fully-qualified name of the parameter, including its namespace.
         :param value: Value of the parameter to declare.
         :param descriptor: Descriptor for the parameter to declare.
@@ -381,6 +384,8 @@ def declare_parameters(
 
         The tuples in the given parameter list shall contain the name for each parameter,
         optionally providing a value and a descriptor.
+        The name and type in the given descriptors are ignored, and should be specified using
+        the name argument to this function and the default value's type instead.
         For each entry in the list, a parameter with a name of "namespace.name"
         will be declared.
         The resulting value for each declared parameter will be returned, considering
@@ -750,7 +755,7 @@ def set_parameters_atomically(self, parameter_list: List[Parameter]) -> SetParam
         allowed for the node, this method will raise a ParameterNotDeclaredException exception.
 
         Parameters are set all at once.
-        If setting a parameter fails due to not being declared, then no parameter will be set set.
+        If setting a parameter fails due to not being declared, then no parameter will be set.
         Either all of the parameters are set or none of them are set.
 
         If undeclared parameters are allowed for the node, then all the parameters will be

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