diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 496fd6e81..fa085d5a4 100644 --- a/rclpy/rclpy/node.py +++ b/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) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 425df4feb..9cab972f7 100644 --- a/rclpy/rclpy/parameter.py +++ b/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()) diff --git a/rclpy/rclpy/parameter_service.py b/rclpy/rclpy/parameter_service.py index 470da452b..4a3b03a00 100644 --- a/rclpy/rclpy/parameter_service.py +++ b/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 diff --git a/rclpy/rclpy/qos.py b/rclpy/rclpy/qos.py index 1f9f6de2e..b984e2a8a 100644 --- a/rclpy/rclpy/qos.py +++ b/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( diff --git a/rclpy/test/test_destruction.py b/rclpy/test/test_destruction.py index 481cb3cd5..ba4cc2e16 100644 --- a/rclpy/test/test_destruction.py +++ b/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)