diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index a7880a0b7..2d2157190 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -209,6 +209,7 @@ if(BUILD_TESTING) test/test_time.py test/test_timer.py test/test_topic_or_service_is_hidden.py + test/test_topic_endpoint_info.py test/test_utilities.py test/test_validate_full_topic_name.py test/test_validate_namespace.py diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index f8cc0dd48..2ca6e3ad5 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -67,6 +67,7 @@ from rclpy.time_source import TimeSource from rclpy.timer import Rate from rclpy.timer import Timer +from rclpy.topic_endpoint_info import TopicEndpointInfo from rclpy.type_support import check_for_type_support from rclpy.utilities import get_default_context from rclpy.validate_full_topic_name import validate_full_topic_name @@ -1642,7 +1643,7 @@ def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: def _count_publishers_or_subscribers(self, topic_name, func): fq_topic_name = expand_topic_name(topic_name, self.get_name(), self.get_namespace()) - validate_topic_name(fq_topic_name) + validate_full_topic_name(fq_topic_name) with self.handle as node_capsule: return func(node_capsule, fq_topic_name) @@ -1681,3 +1682,82 @@ def assert_liveliness(self) -> None: """ with self.handle as capsule: _rclpy.rclpy_assert_liveliness(capsule) + + def _get_info_by_topic( + self, + topic_name: str, + no_mangle: bool, + func: Callable[[object, str, bool], List[Dict]] + ) -> List[TopicEndpointInfo]: + with self.handle as node_capsule: + if no_mangle: + fq_topic_name = topic_name + else: + fq_topic_name = expand_topic_name( + topic_name, self.get_name(), self.get_namespace()) + validate_full_topic_name(fq_topic_name) + fq_topic_name = _rclpy.rclpy_remap_topic_name(node_capsule, fq_topic_name) + + info_dicts = func(node_capsule, fq_topic_name, no_mangle) + infos = [TopicEndpointInfo(**x) for x in info_dicts] + return infos + + def get_publishers_info_by_topic( + self, + topic_name: str, + no_mangle: bool = False + ) -> List[TopicEndpointInfo]: + """ + Return a list of publishers on a given topic. + + The returned parameter is a list of TopicEndpointInfo objects, where each will contain + the node name, node namespace, topic type, topic endpoint's GID, and its QoS profile. + + When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic + name for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps). + When the `no_mangle` parameter is `false`, the provided `topic_name` should follow + ROS topic name conventions. + + `topic_name` may be a relative, private, or fully qualified topic name. + A relative or private topic will be expanded using this node's namespace and name. + The queried `topic_name` is not remapped. + + :param topic_name: the topic_name on which to find the publishers. + :param no_mangle: no_mangle if `true`, `topic_name` needs to be a valid middleware topic + name, otherwise it should be a valid ROS topic name. Defaults to `false`. + :return: a list of TopicEndpointInfo for all the publishers on this topic. + """ + return self._get_info_by_topic( + topic_name, + no_mangle, + _rclpy.rclpy_get_publishers_info_by_topic) + + def get_subscriptions_info_by_topic( + self, + topic_name: str, + no_mangle: bool = False + ) -> List[TopicEndpointInfo]: + """ + Return a list of subscriptions on a given topic. + + The returned parameter is a list of TopicEndpointInfo objects, where each will contain + the node name, node namespace, topic type, topic endpoint's GID, and its QoS profile. + + When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic + name for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps). + When the `no_mangle` parameter is `false`, the provided `topic_name` should follow + ROS topic name conventions. + + `topic_name` may be a relative, private, or fully qualified topic name. + A relative or private topic will be expanded using this node's namespace and name. + The queried `topic_name` is not remapped. + + :param topic_name: the topic_name on which to find the subscriptions. + :param no_mangle: no_mangle if `true`, `topic_name` needs to be a valid middleware topic + name, otherwise it should be a valid ROS topic name. Defaults to `false`. + :return: a list of TopicEndpointInfo for all the subscriptions on this topic. + """ + return self._get_info_by_topic( + topic_name, + no_mangle, + _rclpy.rclpy_get_subscriptions_info_by_topic) diff --git a/rclpy/rclpy/qos.py b/rclpy/rclpy/qos.py index 8775fb1a5..3020b8d35 100644 --- a/rclpy/rclpy/qos.py +++ b/rclpy/rclpy/qos.py @@ -276,6 +276,8 @@ class HistoryPolicy(QoSPolicyEnum): KEEP_LAST = RMW_QOS_POLICY_HISTORY_KEEP_LAST RMW_QOS_POLICY_HISTORY_KEEP_ALL = 2 KEEP_ALL = RMW_QOS_POLICY_HISTORY_KEEP_ALL + RMW_QOS_POLICY_HISTORY_UNKNOWN = 3 + UNKNOWN = RMW_QOS_POLICY_HISTORY_UNKNOWN # Alias with the old name, for retrocompatibility @@ -295,6 +297,8 @@ class ReliabilityPolicy(QoSPolicyEnum): RELIABLE = RMW_QOS_POLICY_RELIABILITY_RELIABLE RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT = 2 BEST_EFFORT = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT + RMW_QOS_POLICY_RELIABILITY_UNKNOWN = 3 + UNKNOWN = RMW_QOS_POLICY_RELIABILITY_UNKNOWN # Alias with the old name, for retrocompatibility @@ -314,6 +318,8 @@ class DurabilityPolicy(QoSPolicyEnum): TRANSIENT_LOCAL = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL RMW_QOS_POLICY_DURABILITY_VOLATILE = 2 VOLATILE = RMW_QOS_POLICY_DURABILITY_VOLATILE + RMW_QOS_POLICY_DURABILITY_UNKNOWN = 3 + UNKNOWN = RMW_QOS_POLICY_DURABILITY_UNKNOWN # Alias with the old name, for retrocompatibility @@ -335,11 +341,15 @@ class LivelinessPolicy(QoSPolicyEnum): MANUAL_BY_NODE = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC = 3 MANUAL_BY_TOPIC = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC + RMW_QOS_POLICY_LIVELINESS_UNKNOWN = 4 + UNKNOWN = RMW_QOS_POLICY_LIVELINESS_UNKNOWN # Alias with the old name, for retrocompatibility QoSLivelinessPolicy = LivelinessPolicy +qos_profile_unknown = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( + 'qos_profile_unknown')) qos_profile_system_default = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( 'qos_profile_system_default')) qos_profile_sensor_data = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( @@ -350,11 +360,12 @@ class LivelinessPolicy(QoSPolicyEnum): 'qos_profile_parameters')) qos_profile_parameter_events = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( 'qos_profile_parameter_events')) -qos_profile_action_status_default = QoSProfile( - **_rclpy_action.rclpy_action_get_rmw_qos_profile('rcl_action_qos_profile_status_default')) +qos_profile_action_status_default = QoSProfile(**_rclpy_action.rclpy_action_get_rmw_qos_profile( + 'rcl_action_qos_profile_status_default')) class QoSPresetProfiles(Enum): + UNKNOWN = qos_profile_unknown SYSTEM_DEFAULT = qos_profile_system_default SENSOR_DATA = qos_profile_sensor_data SERVICES_DEFAULT = qos_profile_services_default diff --git a/rclpy/rclpy/topic_endpoint_info.py b/rclpy/rclpy/topic_endpoint_info.py new file mode 100644 index 000000000..be4ceb773 --- /dev/null +++ b/rclpy/rclpy/topic_endpoint_info.py @@ -0,0 +1,174 @@ +# Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +# +# 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 IntEnum + +from rclpy.qos import QoSPresetProfiles, QoSProfile + + +class TopicEndpointTypeEnum(IntEnum): + """ + Enum for possible types of topic endpoints. + + This enum matches the one defined in rmw/types.h + """ + + INVALID = 0 + PUBLISHER = 1 + SUBSCRIPTION = 2 + + +class TopicEndpointInfo: + """Information on a topic endpoint.""" + + __slots__ = [ + '_node_name', + '_node_namespace', + '_topic_type', + '_endpoint_type', + '_endpoint_gid', + '_qos_profile' + ] + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %r' % kwargs.keys() + + self.node_name = kwargs.get('node_name', '') + self.node_namespace = kwargs.get('node_namespace', '') + self.topic_type = kwargs.get('topic_type', '') + self.endpoint_type = kwargs.get('endpoint_type', TopicEndpointTypeEnum.INVALID) + self.endpoint_gid = kwargs.get('endpoint_gid', []) + self.qos_profile = kwargs.get('qos_profile', QoSPresetProfiles.UNKNOWN.value) + + @property + def node_name(self): + """ + Get field 'node_name'. + + :returns: node_name attribute + :rtype: str + """ + return self._node_name + + @node_name.setter + def node_name(self, value): + assert isinstance(value, str) + self._node_name = value + + @property + def node_namespace(self): + """ + Get field 'node_namespace'. + + :returns: node_namespace attribute + :rtype: str + """ + return self._node_namespace + + @node_namespace.setter + def node_namespace(self, value): + assert isinstance(value, str) + self._node_namespace = value + + @property + def topic_type(self): + """ + Get field 'topic_type'. + + :returns: topic_type attribute + :rtype: str + """ + return self._topic_type + + @topic_type.setter + def topic_type(self, value): + assert isinstance(value, str) + self._topic_type = value + + @property + def endpoint_type(self): + """ + Get field 'endpoint_type'. + + :returns: endpoint_type attribute + :rtype: TopicEndpointTypeEnum + """ + return self._endpoint_type + + @endpoint_type.setter + def endpoint_type(self, value): + if isinstance(value, TopicEndpointTypeEnum): + self._endpoint_type = value + elif isinstance(value, int): + self._endpoint_type = TopicEndpointTypeEnum(value) + else: + assert False + + @property + def endpoint_gid(self): + """ + Get field 'endpoint_gid'. + + :returns: endpoint_gid attribute + :rtype: list + """ + return self._endpoint_gid + + @endpoint_gid.setter + def endpoint_gid(self, value): + assert all(isinstance(x, int) for x in value) + self._endpoint_gid = value + + @property + def qos_profile(self): + """ + Get field 'qos_profile'. + + :returns: qos_profile attribute + :rtype: QoSProfile + """ + return self._qos_profile + + @qos_profile.setter + def qos_profile(self, value): + if isinstance(value, QoSProfile): + self._qos_profile = value + elif isinstance(value, dict): + self._qos_profile = QoSProfile(**value) + else: + assert False + + def __eq__(self, other): + if not isinstance(other, TopicEndpointInfo): + return False + return all( + self.__getattribute__(slot) == other.__getattribute__(slot) + for slot in self.__slots__) + + def __str__(self): + result = 'Node name: %s\n' % self.node_name + result += 'Node namespace: %s\n' % self.node_namespace + result += 'Topic type: %s\n' % self.topic_type + result += 'Endpoint type: %s\n' % self.endpoint_type.name + result += 'GID: %s\n' % '.'.join(format(x, '02x') for x in self.endpoint_gid) + result += 'QoS profile:\n' + result += ' Reliability: %s\n' % self.qos_profile.reliability.name + result += ' Durability: %s\n' % self.qos_profile.durability.name + result += ' Lifespan: %d nanoseconds\n' % self.qos_profile.lifespan.nanoseconds + result += ' Deadline: %d nanoseconds\n' % self.qos_profile.deadline.nanoseconds + result += ' Liveliness: %s\n' % self.qos_profile.liveliness.name + result += ' Liveliness lease duration: %d nanoseconds' % \ + self.qos_profile.liveliness_lease_duration.nanoseconds + return result diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 3d7c73ed9..723886456 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -20,10 +20,11 @@ #include #include #include +#include #include #include -#include #include +#include #include #include #include @@ -31,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -885,6 +887,104 @@ rclpy_count_subscribers(PyObject * Py_UNUSED(self), PyObject * args) return _count_subscribers_publishers(args, "subscribers", rcl_count_subscribers); } +typedef rcl_ret_t (* rcl_get_info_by_topic_func_t)( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rcl_topic_endpoint_info_array_t * info_array); + +static PyObject * +_get_info_by_topic( + PyObject * args, + const char * type, + rcl_get_info_by_topic_func_t rcl_get_info_by_topic) +{ + PyObject * pynode; + const char * topic_name; + int no_mangle; + + if (!PyArg_ParseTuple(args, "Osp", &pynode, &topic_name, &no_mangle)) { + return NULL; + } + + rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + if (!node) { + return NULL; + } + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_topic_endpoint_info_array_t info_array = rcl_get_zero_initialized_topic_endpoint_info_array(); + rcl_ret_t ret = rcl_get_info_by_topic(node, &allocator, topic_name, no_mangle, &info_array); + rcl_ret_t fini_ret; + if (RCL_RET_OK != ret) { + if (RCL_RET_BAD_ALLOC == ret) { + PyErr_Format( + PyExc_MemoryError, "Failed to get information by topic for %s: %s", + type, rcl_get_error_string().str); + } else if (RCL_RET_UNSUPPORTED == ret) { + PyErr_Format( + PyExc_NotImplementedError, "Failed to get information by topic for %s: " + "function not supported by RMW_IMPLEMENTATION", type); + } else { + PyErr_Format( + RCLError, "Failed to get information by topic for %s: %s", + type, rcl_get_error_string().str); + } + rcl_reset_error(); + fini_ret = rcl_topic_endpoint_info_array_fini(&info_array, &allocator); + if (fini_ret != RCL_RET_OK) { + PyErr_Format( + RCLError, "rcl_topic_endpoint_info_array_fini failed: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + return NULL; + } + PyObject * py_info_array = rclpy_convert_to_py_topic_endpoint_info_list(&info_array); + fini_ret = rcl_topic_endpoint_info_array_fini(&info_array, &allocator); + if (RCL_RET_OK != fini_ret) { + PyErr_Format(RCLError, "rcl_topic_endpoint_info_array_fini failed."); + rcl_reset_error(); + return NULL; + } + return py_info_array; +} + +/// Return a list of publishers on a given topic. +/** + * The returned publisher information includes node name, node namespace, topic type, gid, + * and qos profile + * + * \param[in] pynode Capsule pointing to the node to get the namespace from. + * \param[in] topic_name the topic name to get the publishers for. + * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, + * otherwise it should be a valid ROS topic name. + * \return list of publishers + */ +static PyObject * +rclpy_get_publishers_info_by_topic(PyObject * Py_UNUSED(self), PyObject * args) +{ + return _get_info_by_topic(args, "publishers", rcl_get_publishers_info_by_topic); +} + +/// Return a list of subscriptions on a given topic. +/** + * The returned subscription information includes node name, node namespace, topic type, gid, + * and qos profile + * + * \param[in] pynode Capsule pointing to the node to get the namespace from. + * \param[in] topic_name the topic name to get the subscriptions for. + * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, + * otherwise it should be a valid ROS topic name. + * \return list of subscriptions. + */ +static PyObject * +rclpy_get_subscriptions_info_by_topic(PyObject * Py_UNUSED(self), PyObject * args) +{ + return _get_info_by_topic(args, "subscriptions", rcl_get_subscriptions_info_by_topic); +} + + /// Validate a topic name and return error message and index of invalidation. /** * Does not have to be a fully qualified topic name. @@ -1232,7 +1332,7 @@ _expand_topic_name_with_exceptions(const char * topic, const char * node, const * \param[in] topic_name topic string to be expanded * \param[in] node_name name of the node to be used during expansion * \param[in] node_namespace namespace of the node to be used during expansion - * \return expanded node namespace + * \return expanded topic name */ static PyObject * rclpy_expand_topic_name(PyObject * Py_UNUSED(self), PyObject * args) @@ -1261,16 +1361,12 @@ rclpy_expand_topic_name(PyObject * Py_UNUSED(self), PyObject * args) } char * expanded_topic = _expand_topic_name_with_exceptions(topic, node_name, node_namespace); - if (!expanded_topic) { // exception already set return NULL; } PyObject * result = PyUnicode_FromString(expanded_topic); - if (!result) { - return NULL; - } rcl_allocator_t allocator = rcl_get_default_allocator(); allocator.deallocate(expanded_topic, allocator.state); @@ -1278,6 +1374,73 @@ rclpy_expand_topic_name(PyObject * Py_UNUSED(self), PyObject * args) return result; } +static char * +_remap_topic_name_with_exceptions(const rcl_node_t * node_handle, const char * topic_name) +{ + // Get the node options + const rcl_node_options_t * node_options = rcl_node_get_options(node_handle); + if (node_options == NULL) { + return NULL; + } + const rcl_arguments_t * global_args = NULL; + if (node_options->use_global_arguments) { + global_args = &(node_handle->context->global_arguments); + } + + char * remapped_topic = NULL; + rcl_ret_t ret = rcl_remap_topic_name( + &(node_options->arguments), + global_args, + topic_name, + rcl_node_get_name(node_handle), + rcl_node_get_namespace(node_handle), + node_options->allocator, + &remapped_topic); + if (ret != RCL_RET_OK) { + PyErr_Format(PyExc_RuntimeError, "Failed to remap topic name %s", topic_name); + return NULL; + } + + return remapped_topic; +} + +/// Remap a topic name +/** + * Raises ValueError if the capsule is not the correct type + * + * \param[in] pynode Capsule pointing to the node + * \param[in] topic_name topic string to be remapped + * \return remapped topic name + */ +static PyObject * +rclpy_remap_topic_name(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pynode; + const char * topic_name; + + if (!PyArg_ParseTuple(args, "Os", &pynode, &topic_name)) { + return NULL; + } + + const rcl_node_t * node = (const rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + if (node == NULL) { + return NULL; + } + + char * remapped_topic_name = _remap_topic_name_with_exceptions(node, topic_name); + if (remapped_topic_name == NULL) { + return PyUnicode_FromString(topic_name); + } + + PyObject * result = PyUnicode_FromString(remapped_topic_name); + + const rcl_node_options_t * node_options = rcl_node_get_options(node); + rcl_allocator_t allocator = node_options->allocator; + allocator.deallocate(remapped_topic_name, allocator.state); + + return result; +} + /// PyCapsule destructor for publisher static void _rclpy_destroy_publisher(PyObject * pyentity) @@ -1458,8 +1621,8 @@ rclpy_publisher_get_subscription_count(PyObject * Py_UNUSED(self), PyObject * ar } size_t count = 0; - rmw_ret_t ret = rcl_publisher_get_subscription_count(&pub->publisher, &count); - if (RMW_RET_OK != ret) { + rcl_ret_t ret = rcl_publisher_get_subscription_count(&pub->publisher, &count); + if (RCL_RET_OK != ret) { PyErr_Format(RCLError, "%s", rmw_get_error_string().str); rmw_reset_error(); return NULL; @@ -2833,7 +2996,7 @@ rclpy_take_raw(rcl_subscription_t * subscription) } ret = rcl_take_serialized_message(subscription, &msg, NULL, NULL); - if (ret != RMW_RET_OK) { + if (ret != RCL_RET_OK) { PyErr_Format( RCLError, "Failed to take_serialized from a subscription: %s", rcl_get_error_string().str); @@ -3757,6 +3920,8 @@ rclpy_get_rmw_qos_profile(PyObject * Py_UNUSED(self), PyObject * args) pyqos_profile = rclpy_common_convert_to_qos_dict(&rmw_qos_profile_system_default); } else if (0 == strcmp(pyrmw_profile, "qos_profile_services_default")) { pyqos_profile = rclpy_common_convert_to_qos_dict(&rmw_qos_profile_services_default); + } else if (0 == strcmp(pyrmw_profile, "qos_profile_unknown")) { + pyqos_profile = rclpy_common_convert_to_qos_dict(&rmw_qos_profile_unknown); } else if (0 == strcmp(pyrmw_profile, "qos_profile_parameters")) { pyqos_profile = rclpy_common_convert_to_qos_dict(&rmw_qos_profile_parameters); } else if (0 == strcmp(pyrmw_profile, "qos_profile_parameter_events")) { @@ -4877,10 +5042,22 @@ static PyMethodDef rclpy_methods[] = { "rclpy_count_subscribers", rclpy_count_subscribers, METH_VARARGS, "Count subscribers for a topic." }, + { + "rclpy_get_publishers_info_by_topic", rclpy_get_publishers_info_by_topic, METH_VARARGS, + "Get publishers info for a topic." + }, + { + "rclpy_get_subscriptions_info_by_topic", rclpy_get_subscriptions_info_by_topic, METH_VARARGS, + "Get subscriptions info for a topic." + }, { "rclpy_expand_topic_name", rclpy_expand_topic_name, METH_VARARGS, "Expand a topic name." }, + { + "rclpy_remap_topic_name", rclpy_remap_topic_name, METH_VARARGS, + "Remap a topic name." + }, { "rclpy_get_validation_error_for_topic_name", rclpy_get_validation_error_for_topic_name, METH_VARARGS, diff --git a/rclpy/src/rclpy_common/include/rclpy_common/common.h b/rclpy/src/rclpy_common/include/rclpy_common/common.h index 5f450ecde..866b21303 100644 --- a/rclpy/src/rclpy_common/include/rclpy_common/common.h +++ b/rclpy/src/rclpy_common/include/rclpy_common/common.h @@ -132,4 +132,13 @@ RCLPY_COMMON_PUBLIC PyObject * rclpy_convert_to_py(void * message, PyObject * pyclass); +/// Convert a C rmw_topic_endpoint_info_array_t into a Python list. +/** + * \param[in] info_array a pointer to a rmw_topic_endpoint_info_array_t + * \return Python list + */ +RCLPY_COMMON_PUBLIC +PyObject * +rclpy_convert_to_py_topic_endpoint_info_list(const rmw_topic_endpoint_info_array_t * info_array); + #endif // RCLPY_COMMON__COMMON_H_ diff --git a/rclpy/src/rclpy_common/src/common.c b/rclpy/src/rclpy_common/src/common.c index ef47edeaa..0bbfcde2a 100644 --- a/rclpy/src/rclpy_common/src/common.c +++ b/rclpy/src/rclpy_common/src/common.c @@ -358,3 +358,104 @@ rclpy_convert_to_py(void * message, PyObject * pyclass) } return convert(message); } + +PyObject * +_rclpy_convert_to_py_topic_endpoint_info(const rmw_topic_endpoint_info_t * topic_endpoint_info) +{ + PyObject * py_node_name = NULL; + PyObject * py_node_namespace = NULL; + PyObject * py_topic_type = NULL; + PyObject * py_endpoint_type = NULL; + PyObject * py_endpoint_gid = NULL; + PyObject * py_qos_profile = NULL; + PyObject * py_endpoint_info_dict = NULL; + + py_node_name = PyUnicode_FromString(topic_endpoint_info->node_name); + if (!py_node_name) { + goto fail; + } + py_node_namespace = PyUnicode_FromString(topic_endpoint_info->node_namespace); + if (!py_node_namespace) { + goto fail; + } + py_topic_type = PyUnicode_FromString(topic_endpoint_info->topic_type); + if (!py_topic_type) { + goto fail; + } + py_endpoint_type = PyLong_FromUnsignedLong(topic_endpoint_info->endpoint_type); + if (!py_endpoint_type) { + goto fail; + } + + py_endpoint_gid = PyList_New(RMW_GID_STORAGE_SIZE); + if (!py_endpoint_gid) { + goto fail; + } + for (int i = 0; i < RMW_GID_STORAGE_SIZE; i++) { + PyObject * py_val_at_index = PyLong_FromUnsignedLong(topic_endpoint_info->endpoint_gid[i]); + if (!py_val_at_index) { + goto fail; + } + PyList_SET_ITEM(py_endpoint_gid, i, py_val_at_index); + } + + py_qos_profile = rclpy_common_convert_to_qos_dict(&topic_endpoint_info->qos_profile); + if (!py_qos_profile) { + goto fail; + } + + // Create dictionary that represents rmw_topic_endpoint_info_t + py_endpoint_info_dict = PyDict_New(); + if (!py_endpoint_info_dict) { + goto fail; + } + // Populate keyword arguments + // A success returns 0, and a failure returns -1 + int set_result = 0; + set_result += PyDict_SetItemString(py_endpoint_info_dict, "node_name", py_node_name); + set_result += PyDict_SetItemString(py_endpoint_info_dict, "node_namespace", py_node_namespace); + set_result += PyDict_SetItemString(py_endpoint_info_dict, "topic_type", py_topic_type); + set_result += PyDict_SetItemString(py_endpoint_info_dict, "endpoint_type", py_endpoint_type); + set_result += PyDict_SetItemString(py_endpoint_info_dict, "endpoint_gid", py_endpoint_gid); + set_result += PyDict_SetItemString(py_endpoint_info_dict, "qos_profile", py_qos_profile); + if (set_result != 0) { + goto fail; + } + return py_endpoint_info_dict; + +fail: + Py_XDECREF(py_node_name); + Py_XDECREF(py_node_namespace); + Py_XDECREF(py_topic_type); + Py_XDECREF(py_endpoint_type); + Py_XDECREF(py_endpoint_gid); + Py_XDECREF(py_qos_profile); + Py_XDECREF(py_endpoint_info_dict); + return NULL; +} + +PyObject * +rclpy_convert_to_py_topic_endpoint_info_list(const rmw_topic_endpoint_info_array_t * info_array) +{ + if (!info_array) { + return NULL; + } + + PyObject * py_info_array = PyList_New(info_array->count); + if (!py_info_array) { + return NULL; + } + + for (size_t i = 0; i < info_array->count; ++i) { + rmw_topic_endpoint_info_t topic_endpoint_info = info_array->info_array[i]; + PyObject * py_endpoint_info_dict = _rclpy_convert_to_py_topic_endpoint_info( + &topic_endpoint_info); + if (!py_endpoint_info_dict) { + Py_DECREF(py_info_array); + return NULL; + } + // add this dict to the list + PyList_SET_ITEM(py_info_array, i, py_endpoint_info_dict); + } + return py_info_array; +} diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index b699871ce..51e671430 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -25,6 +25,7 @@ from rcl_interfaces.srv import GetParameters import rclpy from rclpy.clock import ClockType +from rclpy.duration import Duration from rclpy.exceptions import InvalidParameterException from rclpy.exceptions import InvalidParameterValueException from rclpy.exceptions import InvalidServiceNameException @@ -35,7 +36,13 @@ from rclpy.executors import SingleThreadedExecutor from rclpy.parameter import Parameter from rclpy.qos import qos_profile_sensor_data +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSLivelinessPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy from rclpy.time_source import USE_SIM_TIME_NAME +from rclpy.utilities import get_rmw_implementation_identifier from test_msgs.msg import BasicTypes TEST_NODE = 'my_node' @@ -171,6 +178,99 @@ def test_node_names_and_namespaces(self): # test that it doesn't raise self.node.get_node_names_and_namespaces() + def assert_qos_equal(self, expected_qos_profile, actual_qos_profile): + # Depth and history are skipped because they are not retrieved. + self.assertEqual( + expected_qos_profile.durability, + actual_qos_profile.durability, + 'Durability is unequal') + self.assertEqual( + expected_qos_profile.reliability, + actual_qos_profile.reliability, + 'Reliability is unequal') + self.assertEqual( + expected_qos_profile.lifespan, + actual_qos_profile.lifespan, + 'lifespan is unequal') + self.assertEqual( + expected_qos_profile.deadline, + actual_qos_profile.deadline, + 'Deadline is unequal') + self.assertEqual( + expected_qos_profile.liveliness, + actual_qos_profile.liveliness, + 'liveliness is unequal') + self.assertEqual( + expected_qos_profile.liveliness_lease_duration, + actual_qos_profile.liveliness_lease_duration, + 'liveliness_lease_duration is unequal') + + @unittest.skipIf(get_rmw_implementation_identifier() != 'rmw_fastrtps_cpp', + 'Implementation is not supported') + def test_get_publishers_subscriptions_info_by_topic(self): + topic_name = 'test_topic_endpoint_info' + fq_topic_name = '{namespace}/{name}'.format(namespace=TEST_NAMESPACE, name=topic_name) + # Lists should be empty + self.assertFalse(self.node.get_publishers_info_by_topic(fq_topic_name)) + self.assertFalse(self.node.get_subscriptions_info_by_topic(fq_topic_name)) + + # Add a publisher + qos_profile = QoSProfile( + depth=10, + history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL, + deadline=Duration(seconds=1, nanoseconds=12345), + lifespan=Duration(seconds=20, nanoseconds=9887665), + reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + liveliness_lease_duration=Duration(seconds=5, nanoseconds=23456), + liveliness=QoSLivelinessPolicy.MANUAL_BY_TOPIC) + self.node.create_publisher(BasicTypes, topic_name, qos_profile) + # List should have one item + publisher_list = self.node.get_publishers_info_by_topic(fq_topic_name) + self.assertEqual(1, len(publisher_list)) + # Subscription list should be empty + self.assertFalse(self.node.get_subscriptions_info_by_topic(fq_topic_name)) + # Verify publisher list has the right data + self.assertEqual(self.node.get_name(), publisher_list[0].node_name) + self.assertEqual(self.node.get_namespace(), publisher_list[0].node_namespace) + self.assertEqual('test_msgs/msg/BasicTypes', publisher_list[0].topic_type) + actual_qos_profile = publisher_list[0].qos_profile + self.assert_qos_equal(qos_profile, actual_qos_profile) + + # Add a subscription + qos_profile2 = QoSProfile( + depth=0, + history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + deadline=Duration(seconds=15, nanoseconds=1678), + lifespan=Duration(seconds=29, nanoseconds=2345), + reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE, + durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE, + liveliness_lease_duration=Duration(seconds=5, nanoseconds=23456), + liveliness=QoSLivelinessPolicy.MANUAL_BY_NODE) + self.node.create_subscription(BasicTypes, topic_name, lambda msg: print(msg), qos_profile2) + # Both lists should have one item + publisher_list = self.node.get_publishers_info_by_topic(fq_topic_name) + subscription_list = self.node.get_subscriptions_info_by_topic(fq_topic_name) + self.assertEqual(1, len(publisher_list)) + self.assertEqual(1, len(subscription_list)) + # Verify subscription list has the right data + self.assertEqual(self.node.get_name(), publisher_list[0].node_name) + self.assertEqual(self.node.get_namespace(), publisher_list[0].node_namespace) + self.assertEqual('test_msgs/msg/BasicTypes', publisher_list[0].topic_type) + self.assertEqual('test_msgs/msg/BasicTypes', subscription_list[0].topic_type) + publisher_qos_profile = publisher_list[0].qos_profile + subscription_qos_profile = subscription_list[0].qos_profile + self.assert_qos_equal(qos_profile, publisher_qos_profile) + self.assert_qos_equal(qos_profile2, subscription_qos_profile) + + # Error cases + with self.assertRaisesRegex(TypeError, 'bad argument type for built-in operation'): + self.node.get_subscriptions_info_by_topic(1) + self.node.get_publishers_info_by_topic(1) + with self.assertRaisesRegex(ValueError, 'is invalid'): + self.node.get_subscriptions_info_by_topic('13') + self.node.get_publishers_info_by_topic('13') + def test_count_publishers_subscribers(self): short_topic_name = 'chatter' fq_topic_name = '%s/%s' % (TEST_NAMESPACE, short_topic_name) diff --git a/rclpy/test/test_qos.py b/rclpy/test/test_qos.py index bc1b0ade2..9014a2eed 100644 --- a/rclpy/test/test_qos.py +++ b/rclpy/test/test_qos.py @@ -105,7 +105,9 @@ def test_invalid_qos(self): def test_policy_short_names(self): # Full test on History to show the mechanism works - assert QoSHistoryPolicy.short_keys() == ['system_default', 'keep_last', 'keep_all'] + assert ( + QoSHistoryPolicy.short_keys() == + ['system_default', 'keep_last', 'keep_all', 'unknown']) assert ( QoSHistoryPolicy.get_from_short_key('system_default') == QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT.value) diff --git a/rclpy/test/test_topic_endpoint_info.py b/rclpy/test/test_topic_endpoint_info.py new file mode 100644 index 000000000..10d81355a --- /dev/null +++ b/rclpy/test/test_topic_endpoint_info.py @@ -0,0 +1,104 @@ +# Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +# +# 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. + +import unittest + +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.qos import QoSProfile +from rclpy.topic_endpoint_info import TopicEndpointInfo, TopicEndpointTypeEnum + + +class TestQosProfile(unittest.TestCase): + + def test_node_name_only_constructor(self): + test_node_name = 'test_string' + + info_for_ref = TopicEndpointInfo() + info_for_ref.node_name = test_node_name + + info_from_ctor = TopicEndpointInfo(node_name=test_node_name) + + self.assertEqual(info_for_ref, info_from_ctor) + self.assertEqual(test_node_name, info_from_ctor.node_name) + + def test_node_namespace_only_constructor(self): + test_node_namespace = 'test_string' + + info_for_ref = TopicEndpointInfo() + info_for_ref.node_namespace = test_node_namespace + + info_from_ctor = TopicEndpointInfo(node_namespace=test_node_namespace) + + self.assertEqual(info_for_ref, info_from_ctor) + self.assertEqual(test_node_namespace, info_from_ctor.node_namespace) + + def test_topic_type_only_constructor(self): + test_topic_type = 'test_string' + + info_for_ref = TopicEndpointInfo() + info_for_ref.topic_type = test_topic_type + + info_from_ctor = TopicEndpointInfo(topic_type=test_topic_type) + + self.assertEqual(info_for_ref, info_from_ctor) + self.assertEqual(test_topic_type, info_from_ctor.topic_type) + + def test_endpoint_type_only_constructor(self): + test_endpoint_type = TopicEndpointTypeEnum.SUBSCRIPTION + + info_for_ref = TopicEndpointInfo() + info_for_ref.endpoint_type = test_endpoint_type + + info_from_ctor = TopicEndpointInfo(endpoint_type=test_endpoint_type) + + self.assertEqual(info_for_ref, info_from_ctor) + self.assertEqual(test_endpoint_type, info_from_ctor.endpoint_type) + + def test_endpoint_gid_only_constructor(self): + test_endpoint_gid = [0, 0, 0, 0, 0] + + info_for_ref = TopicEndpointInfo() + info_for_ref.endpoint_gid = test_endpoint_gid + + info_from_ctor = TopicEndpointInfo(endpoint_gid=test_endpoint_gid) + + self.assertEqual(info_for_ref, info_from_ctor) + self.assertEqual(test_endpoint_gid, info_from_ctor.endpoint_gid) + + def test_qos_profile_only_constructor(self): + test_qos_profile = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile('qos_profile_default')) + + info_for_ref = TopicEndpointInfo() + info_for_ref.qos_profile = test_qos_profile + + info_from_ctor = TopicEndpointInfo(qos_profile=test_qos_profile) + + self.assertEqual(info_for_ref, info_from_ctor) + self.assertEqual(test_qos_profile, info_from_ctor.qos_profile) + + def test_print(self): + actual_info_str = str(TopicEndpointInfo()) + expected_info_str = 'Node name: \n' \ + 'Node namespace: \n' \ + 'Topic type: \n' \ + 'Endpoint type: INVALID\n' \ + 'GID: \n' \ + 'QoS profile:\n' \ + ' Reliability: RMW_QOS_POLICY_RELIABILITY_UNKNOWN\n' \ + ' Durability: RMW_QOS_POLICY_DURABILITY_UNKNOWN\n' \ + ' Lifespan: 0 nanoseconds\n' \ + ' Deadline: 0 nanoseconds\n' \ + ' Liveliness: RMW_QOS_POLICY_LIVELINESS_UNKNOWN\n' \ + ' Liveliness lease duration: 0 nanoseconds' + self.assertEqual(expected_info_str, actual_info_str)