Publish parameter events

rclpy/rclpy/node.py

@@ -14,7 +14,7 @@
 
 import weakref
 
-from rcl_interfaces.msg import SetParametersResult
+from rcl_interfaces.msg import ParameterEvent, SetParametersResult
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.client import Client
 from rclpy.clock import ROSClock
@@ -28,7 +28,8 @@
 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.qos import qos_profile_default, qos_profile_parameter_events
+from rclpy.qos import qos_profile_services_default
 from rclpy.service import Service
 from rclpy.subscription import Subscription
 from rclpy.time_source import TimeSource
@@ -100,6 +101,9 @@ def __init__(
 
         self.__executor_weakref = None
 
+        self._parameter_event_publisher = self.create_publisher(
+            ParameterEvent, 'parameter_events', qos_profile=qos_profile_parameter_events)
+
         if start_parameter_services:
             self._parameter_service = ParameterService(self)
 
@@ -166,8 +170,28 @@ def set_parameters_atomically(self, parameter_list):
             result = SetParametersResult(successful=True)
 
         if result.successful:
+            parameter_event = ParameterEvent()
             for param in parameter_list:
-                self._parameters[param.name] = param
+                if Parameter.Type.NOT_SET == param.type_:
+                    if Parameter.Type.NOT_SET != self.get_parameter(param.name).type_:
+                        # Parameter deleted. (Parameter had value and new value is not set)
+                        parameter_event.deleted_parameters.append(
+                            param.to_parameter_msg())
+                    # Delete any unset parameters regardless of their previous value.
+                    # We don't currently store NOT_SET parameters so this is an extra precaution.
+                    if param.name in self._parameters:
+                        del self._parameters[param.name]
+                else:
+                    if Parameter.Type.NOT_SET == self.get_parameter(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)
+                        parameter_event.changed_parameters.append(
+                            param.to_parameter_msg())
+                    self._parameters[param.name] = param
+            self._parameter_event_publisher.publish(parameter_event)
+
         return result
 
     def _validate_topic_or_service_name(self, topic_or_service_name, *, is_service=False):
@@ -339,6 +363,10 @@ def destroy_node(self):
         if self.handle is None:
             return ret
 
+        # Drop extra reference to parameter event publisher.
+        # It will be destroyed with other publishers below.
+        self._parameter_event_publisher = None
+
         while self.publishers:
             pub = self.publishers.pop()
             _rclpy.rclpy_destroy_node_entity(pub.publisher_handle, self.handle)

rclpy/rclpy/parameter.py

@@ -14,6 +14,7 @@
 
 from enum import Enum
 
+from rcl_interfaces.msg import Parameter as ParameterMsg
 from rcl_interfaces.msg import ParameterDescriptor, ParameterType, ParameterValue
 
 PARAMETER_SEPARATOR_STRING = '.'
@@ -62,28 +63,28 @@ def check(self, parameter_value):
             return False
 
     @classmethod
-    def from_rcl_interface_parameter(cls, rcl_param):
+    def from_parameter_msg(cls, param_msg):
         value = None
-        type_ = Parameter.Type(value=rcl_param.value.type)
+        type_ = Parameter.Type(value=param_msg.value.type)
         if Parameter.Type.BOOL == type_:
-            value = rcl_param.value.bool_value
+            value = param_msg.value.bool_value
         elif Parameter.Type.INTEGER == type_:
-            value = rcl_param.value.integer_value
+            value = param_msg.value.integer_value
         elif Parameter.Type.DOUBLE == type_:
-            value = rcl_param.value.double_value
+            value = param_msg.value.double_value
         elif Parameter.Type.STRING == type_:
-            value = rcl_param.value.string_value
+            value = param_msg.value.string_value
         elif Parameter.Type.BYTE_ARRAY == type_:
-            value = rcl_param.value.byte_array_value
+            value = param_msg.value.byte_array_value
         elif Parameter.Type.BOOL_ARRAY == type_:
-            value = rcl_param.value.bool_array_value
+            value = param_msg.value.bool_array_value
         elif Parameter.Type.INTEGER_ARRAY == type_:
-            value = rcl_param.value.integer_array_value
+            value = param_msg.value.integer_array_value
         elif Parameter.Type.DOUBLE_ARRAY == type_:
-            value = rcl_param.value.double_array_value
+            value = param_msg.value.double_array_value
         elif Parameter.Type.STRING_ARRAY == type_:
-            value = rcl_param.value.string_array_value
-        return cls(rcl_param.name, type_, value)
+            value = param_msg.value.string_array_value
+        return cls(param_msg.name, type_, value)
 
     def __init__(self, name, type_, value=None):
         if not isinstance(type_, Parameter.Type):
@@ -132,3 +133,6 @@ def get_parameter_value(self):
         elif Parameter.Type.STRING_ARRAY == self.type_:
             parameter_value.string_array_value = self.value
         return parameter_value
+
+    def to_parameter_msg(self):
+        return ParameterMsg(name=self.name, value=self.get_parameter_value())

rclpy/rclpy/parameter_service.py

@@ -117,11 +117,11 @@ def _list_parameters_callback(self, request, response):
 
     def _set_parameters_callback(self, request, response):
         for p in request.parameters:
-            param = Parameter.from_rcl_interface_parameter(p)
+            param = Parameter.from_parameter_msg(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])
+        response.result = self._node.set_parameters_atomically([
+            Parameter.from_parameter_msg(p) for p in request.parameters])
         return response

rclpy/rclpy/qos.py

@@ -174,8 +174,6 @@ class QoSDurabilityPolicy(IntEnum):
     'qos_profile_sensor_data')
 qos_profile_services_default = _rclpy.rclpy_get_rmw_qos_profile(
     'qos_profile_services_default')
-# NOTE(mikaelarguedas) the following are defined but not used because
-# parameters are not implemented in Python yet
 qos_profile_parameters = _rclpy.rclpy_get_rmw_qos_profile(
     'qos_profile_parameters')
 qos_profile_parameter_events = _rclpy.rclpy_get_rmw_qos_profile(

rclpy/test/test_destruction.py

@@ -107,10 +107,10 @@ def func_destroy_entities():
     timer  # noqa
     assert 1 == len(node.timers)
     pub1 = node.create_publisher(Primitives, 'pub1_topic')
-    assert 1 == len(node.publishers)
+    assert 2 == len(node.publishers)
     pub2 = node.create_publisher(Primitives, 'pub2_topic')
     pub2  # noqa
-    assert 2 == len(node.publishers)
+    assert 3 == len(node.publishers)
     sub1 = node.create_subscription(
         Primitives, 'sub1_topic', lambda msg: print('Received %r' % msg))
     assert 1 == len(node.subscriptions)
@@ -120,7 +120,7 @@ def func_destroy_entities():
     assert 2 == len(node.subscriptions)
 
     assert node.destroy_publisher(pub1) is True
-    assert 1 == len(node.publishers)
+    assert 2 == len(node.publishers)
 
     assert node.destroy_subscription(sub1) is True
     assert 1 == len(node.subscriptions)