diff --git a/rclpy/rclpy/endpoint_info.py b/rclpy/rclpy/endpoint_info.py new file mode 100644 index 000000000..89f33d8bd --- /dev/null +++ b/rclpy/rclpy/endpoint_info.py @@ -0,0 +1,428 @@ +# Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +# Copyright 2025 Minju Lee (이민주). 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 typing import Any, List, Union + +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.qos import QoSHistoryPolicy, QoSPresetProfiles, QoSProfile +from rclpy.type_hash import TypeHash, TypeHashDictionary + + +class EndpointTypeEnum(IntEnum): + """ + Enum for possible types of topic endpoints. + + This enum matches the one defined in rmw/types.h + """ + + INVALID = 0 + PUBLISHER = 1 + SUBSCRIPTION = 2 + CLIENT = 3 + SERVER = 4 + + +class TopicEndpointInfo: + """Information on a topic endpoint.""" + + __slots__ = [ + '_node_name', + '_node_namespace', + '_topic_type', + '_topic_type_hash', + '_endpoint_type', + '_endpoint_gid', + '_qos_profile' + ] + + def __init__( + self, + node_name: str = '', + node_namespace: str = '', + topic_type: str = '', + topic_type_hash: Union[TypeHash, TypeHashDictionary] = TypeHash(), + endpoint_type: Union[EndpointTypeEnum, int] = EndpointTypeEnum.INVALID, + endpoint_gid: List[int] = [], + qos_profile: Union[QoSProfile, '_rclpy._rmw_qos_profile_dict'] = + QoSPresetProfiles.UNKNOWN.value + ): + self.node_name = node_name + self.node_namespace = node_namespace + self.topic_type = topic_type + self.topic_type_hash = topic_type_hash + self.endpoint_type = endpoint_type + self.endpoint_gid = endpoint_gid + self.qos_profile = qos_profile + + @property + def node_name(self) -> str: + """ + Get field 'node_name'. + + :returns: node_name attribute + """ + return self._node_name + + @node_name.setter + def node_name(self, value: str) -> None: + assert isinstance(value, str) + self._node_name = value + + @property + def node_namespace(self) -> str: + """ + Get field 'node_namespace'. + + :returns: node_namespace attribute + """ + return self._node_namespace + + @node_namespace.setter + def node_namespace(self, value: str) -> None: + assert isinstance(value, str) + self._node_namespace = value + + @property + def topic_type(self) -> str: + """ + Get field 'topic_type'. + + :returns: topic_type attribute + """ + return self._topic_type + + @topic_type.setter + def topic_type(self, value: str) -> None: + assert isinstance(value, str) + self._topic_type = value + + # Has to be marked Any due to mypy#3004. Return type is actually TypeHash + @property + def topic_type_hash(self) -> Any: + """ + Get field 'topic_type_hash'. + + :returns: topic_type_hash attribute + """ + return self._topic_type_hash + + @topic_type_hash.setter + def topic_type_hash(self, value: Union[TypeHash, TypeHashDictionary]) -> None: + if isinstance(value, TypeHash): + self._topic_type_hash = value + elif isinstance(value, dict): + self._topic_type_hash = TypeHash(**value) + else: + assert False + + # Has to be marked Any due to mypy#3004. Return type is actually EndpointTypeEnum + @property + def endpoint_type(self) -> Any: + """ + Get field 'endpoint_type'. + + :returns: endpoint_type attribute + """ + return self._endpoint_type + + @endpoint_type.setter + def endpoint_type(self, value: Union[EndpointTypeEnum, int]) -> None: + if isinstance(value, EndpointTypeEnum): + self._endpoint_type = value + elif isinstance(value, int): + self._endpoint_type = EndpointTypeEnum(value) + else: + assert False + + @property + def endpoint_gid(self) -> List[int]: + """ + Get field 'endpoint_gid'. + + :returns: endpoint_gid attribute + """ + return self._endpoint_gid + + @endpoint_gid.setter + def endpoint_gid(self, value: List[int]) -> None: + assert all(isinstance(x, int) for x in value) + self._endpoint_gid = value + + # Has to be marked Any due to mypy#3004. Return type is actually QoSProfile + @property + def qos_profile(self) -> Any: + """ + Get field 'qos_profile'. + + :returns: qos_profile attribute + """ + return self._qos_profile + + @qos_profile.setter + def qos_profile(self, value: Union[QoSProfile, '_rclpy._rmw_qos_profile_dict']) -> None: + if isinstance(value, QoSProfile): + self._qos_profile = value + elif isinstance(value, dict): + self._qos_profile = QoSProfile(**value) + else: + assert False + + def __eq__(self, other: object) -> bool: + if not isinstance(other, TopicEndpointInfo): + return False + return all( + self.__getattribute__(slot) == other.__getattribute__(slot) + for slot in self.__slots__) + + def __str__(self) -> str: + gid = '.'.join(format(x, '02x') for x in self.endpoint_gid) + if self.qos_profile.history.value != QoSHistoryPolicy.KEEP_LAST: + history_depth_str = self.qos_profile.history.name + else: + history_depth_str = f'{self.qos_profile.history.name} ({self.qos_profile.depth})' + return '\n'.join([ + f'Node name: {self.node_name}', + f'Node namespace: {self.node_namespace}', + f'Topic type: {self.topic_type}', + f'Topic type hash: {self.topic_type_hash}', + f'Endpoint type: {self.endpoint_type.name}', + f'GID: {gid}', + 'QoS profile:', + f' Reliability: {self.qos_profile.reliability.name}', + f' History (Depth): {history_depth_str}', + f' Durability: {self.qos_profile.durability.name}', + f' Lifespan: {self.qos_profile.lifespan}', + f' Deadline: {self.qos_profile.deadline}', + f' Liveliness: {self.qos_profile.liveliness.name}', + f' Liveliness lease duration: {self.qos_profile.liveliness_lease_duration}', + ]) + + +class ServiceEndpointInfo: + """Information on a service endpoint.""" + + __slots__ = [ + '_node_name', + '_node_namespace', + '_service_type', + '_service_type_hash', + '_qos_profiles', + '_endpoint_gids', + '_endpoint_type', + '_endpoint_count' + ] + + def __init__( + self, + node_name: str = '', + node_namespace: str = '', + service_type: str = '', + service_type_hash: Union[TypeHash, TypeHashDictionary] = TypeHash(), + qos_profiles: List[Union[QoSProfile, '_rclpy._rmw_qos_profile_dict']] = [], + endpoint_gids: List[List[int]] = [], + endpoint_type: EndpointTypeEnum = EndpointTypeEnum.INVALID, + endpoint_count: int = 0 + ): + self.node_name = node_name + self.node_namespace = node_namespace + self.service_type = service_type + self.service_type_hash = service_type_hash + self.endpoint_type = endpoint_type + self.endpoint_gids = endpoint_gids + self.qos_profiles = qos_profiles + self.endpoint_count = endpoint_count + + @property + def node_name(self) -> str: + """ + Get field 'node_name'. + + :returns: node_name attribute + """ + return self._node_name + + @node_name.setter + def node_name(self, value: str) -> None: + assert isinstance(value, str) + self._node_name = value + + @property + def node_namespace(self) -> str: + """ + Get field 'node_namespace'. + + :returns: node_namespace attribute + """ + return self._node_namespace + + @node_namespace.setter + def node_namespace(self, value: str) -> None: + assert isinstance(value, str) + self._node_namespace = value + + @property + def service_type(self) -> str: + """ + Get field 'topic_type'. + + :returns: topic_type attribute + """ + return self._service_type + + @service_type.setter + def service_type(self, value: str) -> None: + assert isinstance(value, str) + self._service_type = value + + @property + def service_type_hash(self) -> Any: + """ + Get field 'service_type_hash'. + + :returns: service_type_hash attribute + """ + return self._service_type_hash + + @service_type_hash.setter + def service_type_hash(self, value: Union[TypeHash, TypeHashDictionary]) -> None: + if isinstance(value, TypeHash): + self._service_type_hash = value + elif isinstance(value, dict): + self._service_type_hash = TypeHash(**value) + else: + assert False + + @property + def endpoint_type(self) -> EndpointTypeEnum: + """ + Get field 'endpoint_type'. + + :returns: endpoint_type attribute + """ + return self._endpoint_type + + @endpoint_type.setter + def endpoint_type(self, value: Union[EndpointTypeEnum, int]) -> None: + if isinstance(value, EndpointTypeEnum): + self._endpoint_type = value + elif isinstance(value, int): + self._endpoint_type = EndpointTypeEnum(value) + else: + assert False + + @property + def endpoint_count(self) -> int: + """ + Get field 'node_name'. + + :returns: node_name attribute + """ + return self._endpoint_count + + @endpoint_count.setter + def endpoint_count(self, value: int) -> None: + assert isinstance(value, int) + self._endpoint_count = value + + @property + def endpoint_gids(self) -> List[List[int]]: + """ + Get field 'endpoint_gids'. + + :returns: endpoint_gids attribute + """ + return self._endpoint_gids + + @endpoint_gids.setter + def endpoint_gids(self, value: List[List[int]]) -> None: + assert isinstance(value, list) + assert all(isinstance(x, list) and all(isinstance(i, int) for i in x) for x in value) + self._endpoint_gids = value + + @property + def qos_profiles(self) -> List[QoSProfile]: + """ + Get field 'qos_profile'. + + :returns: qos_profile attribute + """ + return self._qos_profiles + + @qos_profiles.setter + def qos_profiles(self, value: List[Union[QoSProfile, '_rclpy.rmw_qos_profile_dict']]) -> None: + assert isinstance(value, list) + self._qos_profiles = [v if isinstance(v, QoSProfile) else QoSProfile(**v) for v in value] + + def __eq__(self, other: object) -> bool: + if not isinstance(other, ServiceEndpointInfo): + return False + return all( + self.__getattribute__(slot) == other.__getattribute__(slot) + for slot in self.__slots__) + + def __str__(self) -> str: + gids = [] + for endpoint_gid in self.endpoint_gids: + gid = '.'.join(format(x, '02x') for x in endpoint_gid) + gids.append(gid) + + def format_qos(qos, indent=' ') -> str: + return '\n'.join([ + f'{indent}Reliability: {qos.reliability.name}', + f'{indent}History (Depth): {qos.history.name} ({qos.depth})', + f'{indent}Durability: {qos.durability.name}', + f'{indent}Lifespan: {qos.lifespan}', + f'{indent}Deadline: {qos.deadline}', + f'{indent}Liveliness: {qos.liveliness.name}', + f'{indent}Liveliness lease duration: {qos.liveliness_lease_duration}' + ]) + info_lines = [ + f'Node name: {self.node_name}', + f'Node namespace: {self.node_namespace}', + f'Service type: {self.service_type}', + f'Service type hash: {self.service_type_hash}', + f'Endpoint type: {self.endpoint_type.name}', + f'Endpoint count: {self.endpoint_count}', + ] + + if 1 == self.endpoint_count: + info_lines.append(f'GID: {gids[0]}') + info_lines.append('QoS profile:') + info_lines.append(format_qos(self.qos_profiles[0])) + elif self.endpoint_type == EndpointTypeEnum.CLIENT: + info_lines += [ + 'GIDs:', + f' - Request Writer : {gids[1]}', + f' - Response Reader : {gids[0]}', + 'QoS profiles:', + ' - Request Writer :', + format_qos(self.qos_profiles[1], indent=' '), + ' - Response Reader :', + format_qos(self.qos_profiles[0], indent=' '), + ] + else: + info_lines += [ + 'GIDs:', + f' - Request Reader : {gids[0]}', + f' - Response Writer : {gids[1]}', + 'QoS profiles:', + ' - Request Reader :', + format_qos(self.qos_profiles[0], indent=' '), + ' - Response Writer :', + format_qos(self.qos_profiles[1], indent=' '), + ] + + return '\n'.join(info_lines) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 38801d6c2..e3b208d51 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -52,6 +52,7 @@ from rclpy.clock import ROSClock from rclpy.constants import S_TO_NS from rclpy.context import Context +from rclpy.endpoint_info import ServiceEndpointInfo, TopicEndpointInfo from rclpy.event_handler import PublisherEventCallbacks from rclpy.event_handler import SubscriptionEventCallbacks from rclpy.exceptions import InvalidHandle @@ -86,7 +87,6 @@ from rclpy.time_source import TimeSource from rclpy.timer import Rate from rclpy.timer import Timer, TimerInfo -from rclpy.topic_endpoint_info import TopicEndpointInfo from rclpy.type_description_service import TypeDescriptionService from rclpy.type_support import check_is_valid_msg_type from rclpy.type_support import check_is_valid_srv_type @@ -2347,6 +2347,96 @@ def get_subscriptions_info_by_topic( no_mangle, _rclpy.rclpy_get_subscriptions_info_by_topic) + def _get_info_by_service( + self, + service_name: str, + no_mangle: bool, + func: Callable[[_rclpy.Node, str, bool], List['_rclpy.ServiceEndpointInfoDict']] + ) -> List[ServiceEndpointInfo]: + with self.handle: + if no_mangle: + fq_topic_name = service_name + else: + fq_topic_name = expand_topic_name( + service_name, self.get_name(), self.get_namespace()) + validate_full_topic_name(fq_topic_name) + fq_topic_name = _rclpy.rclpy_remap_topic_name(self.handle, fq_topic_name) + info_dicts = func(self.handle, fq_topic_name, no_mangle) + infos = [ServiceEndpointInfo(**x) for x in info_dicts] + return infos + + def get_clients_info_by_service( + self, + service_name: str, + no_mangle: bool = False + ) -> List[ServiceEndpointInfo]: + """ + Return a list of clients on a given service. + + The returned parameter is a list of ServiceEndpointInfo objects, where each will contain + the node name, node namespace, service type, service endpoint's GIDs, and its QoS profiles. + + When the ``no_mangle`` parameter is ``True``, the provided ``service_name`` should be a + valid service name for the middleware (useful when combining ROS with native middleware + apps). When the ``no_mangle`` parameter is ``False``,the provided + ``service_name`` should follow ROS service name conventions. + In DDS-based RMWs, services are implemented as topics with mangled + names (e.g., `rq/my_serviceRequest` and `rp/my_serviceReply`), so `no_mangle = true` is not + supported and will result in an error. Use `get_subscriptions_info_by_topic` or + get_publishers_info_by_topic` for unmangled topic queries in such cases. Other RMWs + (e.g., Zenoh) may support `no_mangle = true` if they natively handle + services without topic-based + + ``service_name`` may be a relative, private, or fully qualified service name. + A relative or private service will be expanded using this node's namespace and name. + The queried ``service_name`` is not remapped. + + :param service_name: The service_name on which to find the clients. + :param no_mangle: If ``True``, `service_name` needs to be a valid middleware service + name, otherwise it should be a valid ROS service name. Defaults to ``False``. + :return: A list of ServiceEndpointInfo for all the clients on this service. + """ + return self._get_info_by_service( + service_name, + no_mangle, + _rclpy.rclpy_get_clients_info_by_service) + + def get_servers_info_by_service( + self, + service_name: str, + no_mangle: bool = False + ) -> List[ServiceEndpointInfo]: + """ + Return a list of servers on a given service. + + The returned parameter is a list of ServiceEndpointInfo objects, where each will contain + the node name, node namespace, service type, service endpoint's GIDs, and its QoS profiles. + + When the ``no_mangle`` parameter is ``True``, the provided ``service_name`` should be a + valid service name for the middleware (useful when combining ROS with native middleware + apps). When the ``no_mangle`` parameter is ``False``,the provided + ``service_name`` should follow ROS service name conventions. + In DDS-based RMWs, services are implemented as topics with mangled + names (e.g., `rq/my_serviceRequest` and `rp/my_serviceReply`), so `no_mangle = true` is not + supported and will result in an error. Use `get_subscriptions_info_by_topic` or + get_publishers_info_by_topic` for unmangled topic queries in such cases. Other RMWs + (e.g., Zenoh) may support `no_mangle = true` if they natively handle + services without topic-based + + ``service_name`` may be a relative, private, or fully qualified service name. + A relative or private service will be expanded using this node's namespace and name. + The queried ``service_name`` is not remapped. + + :param service_name: The service_name on which to find the servers. + :param no_mangle: If ``True``, `service_name` needs to be a valid middleware service + name, otherwise it should be a valid ROS service name. Defaults to ``False``. + :return: A list of ServiceEndpointInfo for all the servers on this service. + """ + return self._get_info_by_service( + service_name, + no_mangle, + _rclpy.rclpy_get_servers_info_by_service) + def wait_for_node( self, fully_qualified_node_name: str, diff --git a/rclpy/rclpy/topic_endpoint_info.py b/rclpy/rclpy/topic_endpoint_info.py deleted file mode 100644 index 28f766fd3..000000000 --- a/rclpy/rclpy/topic_endpoint_info.py +++ /dev/null @@ -1,208 +0,0 @@ -# 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 typing import Any, List, Union - -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.qos import QoSHistoryPolicy, QoSPresetProfiles, QoSProfile -from rclpy.type_hash import TypeHash, TypeHashDictionary - - -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', - '_topic_type_hash', - '_endpoint_type', - '_endpoint_gid', - '_qos_profile' - ] - - def __init__( - self, - node_name: str = '', - node_namespace: str = '', - topic_type: str = '', - topic_type_hash: Union[TypeHash, TypeHashDictionary] = TypeHash(), - endpoint_type: Union[TopicEndpointTypeEnum, int] = TopicEndpointTypeEnum.INVALID, - endpoint_gid: List[int] = [], - qos_profile: Union[QoSProfile, '_rclpy._rmw_qos_profile_dict'] = - QoSPresetProfiles.UNKNOWN.value - ): - self.node_name = node_name - self.node_namespace = node_namespace - self.topic_type = topic_type - self.topic_type_hash = topic_type_hash - self.endpoint_type = endpoint_type - self.endpoint_gid = endpoint_gid - self.qos_profile = qos_profile - - @property - def node_name(self) -> str: - """ - Get field 'node_name'. - - :returns: node_name attribute - """ - return self._node_name - - @node_name.setter - def node_name(self, value: str) -> None: - assert isinstance(value, str) - self._node_name = value - - @property - def node_namespace(self) -> str: - """ - Get field 'node_namespace'. - - :returns: node_namespace attribute - """ - return self._node_namespace - - @node_namespace.setter - def node_namespace(self, value: str) -> None: - assert isinstance(value, str) - self._node_namespace = value - - @property - def topic_type(self) -> str: - """ - Get field 'topic_type'. - - :returns: topic_type attribute - """ - return self._topic_type - - @topic_type.setter - def topic_type(self, value: str) -> None: - assert isinstance(value, str) - self._topic_type = value - - # Has to be marked Any due to mypy#3004. Return type is actually TypeHash - @property - def topic_type_hash(self) -> Any: - """ - Get field 'topic_type_hash'. - - :returns: topic_type_hash attribute - """ - return self._topic_type_hash - - @topic_type_hash.setter - def topic_type_hash(self, value: Union[TypeHash, TypeHashDictionary]) -> None: - if isinstance(value, TypeHash): - self._topic_type_hash = value - elif isinstance(value, dict): - self._topic_type_hash = TypeHash(**value) - else: - assert False - - # Has to be marked Any due to mypy#3004. Return type is actually TopicEndpointTypeEnum - @property - def endpoint_type(self) -> Any: - """ - Get field 'endpoint_type'. - - :returns: endpoint_type attribute - """ - return self._endpoint_type - - @endpoint_type.setter - def endpoint_type(self, value: Union[TopicEndpointTypeEnum, int]) -> None: - 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) -> List[int]: - """ - Get field 'endpoint_gid'. - - :returns: endpoint_gid attribute - """ - return self._endpoint_gid - - @endpoint_gid.setter - def endpoint_gid(self, value: List[int]) -> None: - assert all(isinstance(x, int) for x in value) - self._endpoint_gid = value - - # Has to be marked Any due to mypy#3004. Return type is actually QoSProfile - @property - def qos_profile(self) -> Any: - """ - Get field 'qos_profile'. - - :returns: qos_profile attribute - """ - return self._qos_profile - - @qos_profile.setter - def qos_profile(self, value: Union[QoSProfile, '_rclpy._rmw_qos_profile_dict']) -> None: - if isinstance(value, QoSProfile): - self._qos_profile = value - elif isinstance(value, dict): - self._qos_profile = QoSProfile(**value) - else: - assert False - - def __eq__(self, other: object) -> bool: - if not isinstance(other, TopicEndpointInfo): - return False - return all( - self.__getattribute__(slot) == other.__getattribute__(slot) - for slot in self.__slots__) - - def __str__(self) -> str: - gid = '.'.join(format(x, '02x') for x in self.endpoint_gid) - if self.qos_profile.history.value != QoSHistoryPolicy.KEEP_LAST: - history_depth_str = self.qos_profile.history.name - else: - history_depth_str = f'{self.qos_profile.history.name} ({self.qos_profile.depth})' - return '\n'.join([ - f'Node name: {self.node_name}', - f'Node namespace: {self.node_namespace}', - f'Topic type: {self.topic_type}', - f'Topic type hash: {self.topic_type_hash}', - f'Endpoint type: {self.endpoint_type.name}', - f'GID: {gid}', - 'QoS profile:', - f' Reliability: {self.qos_profile.reliability.name}', - f' History (Depth): {history_depth_str}', - f' Durability: {self.qos_profile.durability.name}', - f' Lifespan: {self.qos_profile.lifespan}', - f' Deadline: {self.qos_profile.deadline}', - f' Liveliness: {self.qos_profile.liveliness.name}', - f' Liveliness lease duration: {self.qos_profile.liveliness_lease_duration}', - ]) diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index f33fdaa2f..1bf590325 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -199,6 +199,14 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { "rclpy_get_subscriptions_info_by_topic", &rclpy::graph_get_subscriptions_info_by_topic, "Get subscriptions info for a topic."); + m.def( + "rclpy_get_clients_info_by_service", + &rclpy::graph_get_clients_info_by_service, + "Get clients info for a service."); + m.def( + "rclpy_get_servers_info_by_service", + &rclpy::graph_get_servers_info_by_service, + "Get servers info for a service."); m.def( "rclpy_get_service_names_and_types", &rclpy::graph_get_service_names_and_types, diff --git a/rclpy/src/rclpy/graph.cpp b/rclpy/src/rclpy/graph.cpp index b66cba563..16963731d 100644 --- a/rclpy/src/rclpy/graph.cpp +++ b/rclpy/src/rclpy/graph.cpp @@ -277,4 +277,70 @@ graph_get_subscriptions_info_by_topic( rcl_get_subscriptions_info_by_topic); } +typedef rcl_ret_t (* rcl_get_info_by_service_func_t)( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rcl_service_endpoint_info_array_t * info_array); + + +py::list +_get_info_by_service( + Node & node, + const char * service_name, + bool no_mangle, + const char * type, + rcl_get_info_by_service_func_t rcl_get_info_by_service) +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_service_endpoint_info_array_t info_array = + rcl_get_zero_initialized_service_endpoint_info_array(); + + RCPPUTILS_SCOPE_EXIT( + { + rcl_ret_t fini_ret = rcl_service_endpoint_info_array_fini(&info_array, &allocator); + if (RCL_RET_OK != fini_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " + "rcl_service_endpoint_info_array_fini failed: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + rcl_reset_error(); + } + }); + + rcl_ret_t ret = rcl_get_info_by_service( + node.rcl_ptr(), &allocator, service_name, no_mangle, &info_array); + if (RCL_RET_OK != ret) { + if (RCL_RET_UNSUPPORTED == ret) { + throw NotImplementedError( + std::string("Failed to get information by service for ") + + type + ": function not supported by RMW_IMPLEMENTATION"); + } + throw RCLError( + std::string("Failed to get information by service for ") + type); + } + + return convert_to_py_service_endpoint_info_list(&info_array); +} + +py::list +graph_get_clients_info_by_service( + Node & node, const char * service_name, bool no_mangle) +{ + return _get_info_by_service( + node, service_name, no_mangle, "clients", + rcl_get_clients_info_by_service); +} + +py::list +graph_get_servers_info_by_service( + Node & node, const char * service_name, bool no_mangle) +{ + return _get_info_by_service( + node, service_name, no_mangle, "servers", + rcl_get_servers_info_by_service); +} + } // namespace rclpy diff --git a/rclpy/src/rclpy/graph.hpp b/rclpy/src/rclpy/graph.hpp index 9fcf3c5a9..2cab27355 100644 --- a/rclpy/src/rclpy/graph.hpp +++ b/rclpy/src/rclpy/graph.hpp @@ -164,6 +164,42 @@ py::list graph_get_subscriptions_info_by_topic( Node & node, const char * topic_name, bool no_mangle); +/// Return a list of clients on a given service. +/** + * The returned clients information includes node name, node namespace, service type, + * service type_hash, endpoint_count, gids, and qos profiles. + * + * Raises NotImplementedError if the call is not supported by RMW + * Raises RCLError if there is an rcl error + * + * \param[in] node node to get service clients info + * \param[in] service_name the service name to get the clients for. + * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware service name, + * otherwise it should be a valid ROS service name. + * \return list of clients. + */ +py::list +graph_get_clients_info_by_service( + Node & node, const char * service_name, bool no_mangle); + +/// Return a list of servers on a given service. +/** + * The returned servers information includes node name, node namespace, service type, + * service type_hash, endpoint_count, gids, and qos profiles. + * + * Raises NotImplementedError if the call is not supported by RMW + * Raises RCLError if there is an rcl error + * + * \param[in] node node to get service servers info + * \param[in] service_name the service name to get the servers for. + * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware service name, + * otherwise it should be a valid ROS service name. + * \return list of servers. + */ +py::list +graph_get_servers_info_by_service( + Node & node, const char * service_name, bool no_mangle); + } // namespace rclpy #endif // RCLPY__GRAPH_HPP_ diff --git a/rclpy/src/rclpy/utils.cpp b/rclpy/src/rclpy/utils.cpp index 684b067c5..6232a550d 100644 --- a/rclpy/src/rclpy/utils.cpp +++ b/rclpy/src/rclpy/utils.cpp @@ -334,6 +334,62 @@ convert_to_py_topic_endpoint_info_list(const rmw_topic_endpoint_info_array_t * i return py_info_array; } +py::dict +_convert_to_py_service_endpoint_info(const rmw_service_endpoint_info_t * service_endpoint_info) +{ + py::list py_endpoint_gids; + for(size_t c = 0; c < service_endpoint_info->endpoint_count; c++) { + py::list py_endpoint_gid = py::list(RMW_GID_STORAGE_SIZE); + for (size_t i = 0; i < RMW_GID_STORAGE_SIZE; i++) { + py_endpoint_gid[i] = py::int_(service_endpoint_info->endpoint_gids[c][i]); + } + py_endpoint_gids.append(py_endpoint_gid); + } + // Create dictionary that represents rmw_service_endpoint_info_t + py::dict py_endpoint_info_dict; + // Populate keyword arguments + // A success returns 0, and a failure returns -1 + py_endpoint_info_dict["node_name"] = py::str(service_endpoint_info->node_name); + py_endpoint_info_dict["node_namespace"] = py::str(service_endpoint_info->node_namespace); + py_endpoint_info_dict["service_type"] = py::str(service_endpoint_info->service_type); + py_endpoint_info_dict["service_type_hash"] = + convert_to_type_hash_dict(&service_endpoint_info->service_type_hash); + py_endpoint_info_dict["qos_profiles"] = py::list(); + py_endpoint_info_dict["endpoint_gids"] = py::list(); + + py::list qos_profiles_list; + // Append values to the lists + for (size_t i = 0; i < service_endpoint_info->endpoint_count; i++) { + qos_profiles_list.append( + convert_to_qos_dict(&service_endpoint_info->qos_profiles[i]) + ); + } + py_endpoint_info_dict["qos_profiles"] = qos_profiles_list; + py_endpoint_info_dict["endpoint_gids"] = py_endpoint_gids; + py_endpoint_info_dict["endpoint_type"] = + py::int_(static_cast(service_endpoint_info->endpoint_type)); + py_endpoint_info_dict["endpoint_count"] = py::int_(service_endpoint_info->endpoint_count); + + return py_endpoint_info_dict; +} + +py::list +convert_to_py_service_endpoint_info_list(const rmw_service_endpoint_info_array_t * info_array) +{ + if (!info_array) { + throw std::runtime_error("rmw_service_endpoint_info_array_t pointer is empty"); + } + + py::list py_info_array(info_array->size); + + for (size_t i = 0; i < info_array->size; ++i) { + rmw_service_endpoint_info_t service_endpoint_info = info_array->info_array[i]; + // add this dict to the list + py_info_array[i] = _convert_to_py_service_endpoint_info(&service_endpoint_info); + } + return py_info_array; +} + static py::object _convert_rmw_time_to_py_duration(const rmw_time_t * duration) diff --git a/rclpy/src/rclpy/utils.hpp b/rclpy/src/rclpy/utils.hpp index 3ab5833f9..39e6b5ddf 100644 --- a/rclpy/src/rclpy/utils.hpp +++ b/rclpy/src/rclpy/utils.hpp @@ -142,6 +142,16 @@ rclpy_action_get_rmw_qos_profile(const char * rmw_profile); py::list convert_to_py_topic_endpoint_info_list(const rmw_topic_endpoint_info_array_t * info_array); +/// Convert a C rmw_service_endpoint_info_array_t into a Python list. +/** + * Raises RuntimeError if the rmw_profile profile is null. + * + * \param[in] info_array a pointer to a rmw_service_endpoint_info_array_t + * \return Python list + */ +py::list +convert_to_py_service_endpoint_info_list(const rmw_service_endpoint_info_array_t * info_array); + /// Convert a C rmw_qos_profile_t into a Python dictionary with qos profile args. /** * \param[in] qos_profile Pointer to a rmw_qos_profile_t to convert diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index 8519a5fc7..f542e65b7 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -38,6 +38,7 @@ from rclpy.clock_type import ClockType import rclpy.context from rclpy.duration import Duration +from rclpy.endpoint_info import EndpointTypeEnum from rclpy.exceptions import InvalidParameterException from rclpy.exceptions import InvalidParameterTypeException from rclpy.exceptions import InvalidParameterValueException @@ -59,6 +60,7 @@ from rclpy.type_description_service import START_TYPE_DESCRIPTION_SERVICE_PARAM from rclpy.utilities import get_rmw_implementation_identifier from test_msgs.msg import BasicTypes +from test_msgs.srv import Empty TEST_NODE = 'my_node' TEST_NAMESPACE = '/my_ns' @@ -328,6 +330,79 @@ def test_get_publishers_subscriptions_info_by_topic(self) -> None: self.node.get_subscriptions_info_by_topic('13') self.node.get_publishers_info_by_topic('13') + def test_get_clients_servers_info_by_service(self): + service_name = 'test_service_endpoint_info' + fq_service_name = '{namespace}/{name}'.format(namespace=TEST_NAMESPACE, name=service_name) + # Lists should be empty + self.assertFalse(self.node.get_clients_info_by_service(fq_service_name)) + self.assertFalse(self.node.get_servers_info_by_service(fq_service_name)) + + # Add a client + qos_profile = QoSProfile( + depth=10, + history=QoSHistoryPolicy.KEEP_ALL, + deadline=Duration(seconds=1, nanoseconds=12345), + lifespan=Duration(seconds=20, nanoseconds=9887665), + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + liveliness_lease_duration=Duration(seconds=5, nanoseconds=23456), + liveliness=QoSLivelinessPolicy.MANUAL_BY_TOPIC) + self.node.create_client(Empty, service_name, qos_profile=qos_profile) + # List should have at least one item + client_list = self.node.get_clients_info_by_service(fq_service_name) + self.assertGreaterEqual(len(client_list), 1) + # Server list should be empty + self.assertFalse(self.node.get_servers_info_by_service(fq_service_name)) + + # Verify client list has the right data + self.assertEqual(self.node.get_name(), client_list[0].node_name) + self.assertEqual(self.node.get_namespace(), client_list[0].node_namespace) + self.assertEqual('test_msgs/srv/Empty', client_list[0].service_type) + self.assertTrue(client_list[0].endpoint_count == 1 or client_list[0].endpoint_count == 2) + self.assertEqual(client_list[0].endpoint_type, EndpointTypeEnum.CLIENT) + for i in range(client_list[0].endpoint_count): + actual_qos_profile = client_list[0].qos_profiles[i] + self.assert_qos_equal(qos_profile, actual_qos_profile, is_publisher=False) + + # Add a server + qos_profile2 = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + deadline=Duration(seconds=15, nanoseconds=1678), + lifespan=Duration(seconds=29, nanoseconds=2345), + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE, + liveliness_lease_duration=Duration(seconds=5, nanoseconds=23456), + liveliness=QoSLivelinessPolicy.AUTOMATIC) + self.node.create_service( + Empty, + service_name, + lambda msg: print(msg), + qos_profile=qos_profile2 + ) + # Both lists should have at least one item + client_list = self.node.get_clients_info_by_service(fq_service_name) + server_list = self.node.get_servers_info_by_service(fq_service_name) + self.assertGreaterEqual(len(client_list), 1) + self.assertGreaterEqual(len(server_list), 1) + # Verify server list has the right data + self.assertEqual(self.node.get_name(), server_list[0].node_name) + self.assertEqual(self.node.get_namespace(), server_list[0].node_namespace) + self.assertEqual('test_msgs/srv/Empty', server_list[0].service_type) + self.assertTrue(server_list[0].endpoint_count == 1 or server_list[0].endpoint_count == 2) + self.assertEqual(server_list[0].endpoint_type, EndpointTypeEnum.SERVER) + for i in range(server_list[0].endpoint_count): + actual_qos_profile = server_list[0].qos_profiles[i] + self.assert_qos_equal(qos_profile2, actual_qos_profile, is_publisher=False) + + # Error cases + with self.assertRaises(TypeError): + self.node.get_clients_info_by_service(1) + self.node.get_servers_info_by_service(1) + with self.assertRaisesRegex(ValueError, 'is invalid'): + self.node.get_clients_info_by_service('13') + self.node.get_servers_info_by_service('13') + def test_count_publishers_subscribers(self) -> None: short_topic_name = 'chatter' fq_topic_name = '%s/%s' % (TEST_NAMESPACE, short_topic_name) diff --git a/rclpy/test/test_topic_endpoint_info.py b/rclpy/test/test_topic_endpoint_info.py index 10db6e321..af4dc352c 100644 --- a/rclpy/test/test_topic_endpoint_info.py +++ b/rclpy/test/test_topic_endpoint_info.py @@ -14,9 +14,9 @@ import unittest +from rclpy.endpoint_info import EndpointTypeEnum, TopicEndpointInfo 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): @@ -55,7 +55,7 @@ def test_topic_type_only_constructor(self) -> None: self.assertEqual(test_topic_type, info_from_ctor.topic_type) def test_endpoint_type_only_constructor(self) -> None: - test_endpoint_type = TopicEndpointTypeEnum.SUBSCRIPTION + test_endpoint_type = EndpointTypeEnum.SUBSCRIPTION info_for_ref = TopicEndpointInfo() info_for_ref.endpoint_type = test_endpoint_type