From 3294bf1e28ab52119817d37cc5eb41667e2acf70 Mon Sep 17 00:00:00 2001 From: "Minju, Lee" Date: Tue, 25 Jun 2024 21:44:25 +0900 Subject: [PATCH] add : get clients, servers info Signed-off-by: Minju, Lee --- rclpy/rclpy/node.py | 60 +++++++++++++++++++++ rclpy/src/rclpy/_rclpy_pybind11.cpp | 8 +++ rclpy/src/rclpy/graph.cpp | 18 +++++++ rclpy/src/rclpy/graph.hpp | 36 +++++++++++++ rclpy/test/test_node.py | 82 +++++++++++++++++++++++++++++ 5 files changed, 204 insertions(+) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 3b7fd37b9..c456da6aa 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -2280,6 +2280,66 @@ def get_subscriptions_info_by_topic( no_mangle, _rclpy.rclpy_get_subscriptions_info_by_topic) + def get_clients_info_by_service( + self, + service_name: str, + no_mangle: bool = False + ) -> List[TopicEndpointInfo]: + """ + Return a list of clients on a given service. + + The returned parameter is a list of TopicEndpointInfo objects, where each will contain + the node name, node namespace, service type, service endpoint's GID, and its QoS profile. + + 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 + (e.g. DDS) apps). When the ``no_mangle`` parameter is ``False``,the provided + ``service_name`` should follow ROS service name conventions. + + ``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 TopicEndpointInfo for all the clients on this service. + """ + return self._get_info_by_topic( + 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[TopicEndpointInfo]: + """ + Return a list of servers on a given service. + + The returned parameter is a list of TopicEndpointInfo objects, where each will contain + the node name, node namespace, service type, service endpoint's GID, and its QoS profile. + + 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 + (e.g. DDS) apps). When the ``no_mangle`` parameter is ``False``,the provided + ``service_name`` should follow ROS service name conventions. + + ``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 TopicEndpointInfo for all the servers on this service. + """ + return self._get_info_by_topic( + 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/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 4da96a5d5..3834568e0 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -198,6 +198,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..6256b6676 100644 --- a/rclpy/src/rclpy/graph.cpp +++ b/rclpy/src/rclpy/graph.cpp @@ -277,4 +277,22 @@ graph_get_subscriptions_info_by_topic( rcl_get_subscriptions_info_by_topic); } +py::list +graph_get_clients_info_by_service( + Node & node, const char * service_name, bool no_mangle) +{ + return _get_info_by_topic( + 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_topic( + 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..5890cdd04 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, gid, + * and qos profile. + * + * 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, gid, + * and qos profile. + * + * 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/test/test_node.py b/rclpy/test/test_node.py index dfd9ad9be..be38a59db 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -51,9 +51,11 @@ from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy from rclpy.time_source import USE_SIM_TIME_NAME +from rclpy.topic_endpoint_info import TopicEndpointTypeEnum 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' @@ -311,6 +313,86 @@ def test_get_publishers_subscriptions_info_by_topic(self): 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 + for client in client_list: + self.assertEqual(self.node.get_name(), client.node_name) + self.assertEqual(self.node.get_namespace(), client.node_namespace) + assert 'test_msgs/srv/Empty' in client.topic_type + if 'test_msgs/srv/Empty_Request' == client.topic_type: + actual_qos_profile = client.qos_profile + assert client.endpoint_type == TopicEndpointTypeEnum.PUBLISHER + self.assert_qos_equal(qos_profile, actual_qos_profile, is_publisher=True) + elif 'test_msgs/srv/Empty_Response' == client.topic_type: + actual_qos_profile = client.qos_profile + assert client.endpoint_type == TopicEndpointTypeEnum.SUBSCRIPTION + 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 + for server in server_list: + self.assertEqual(self.node.get_name(), server.node_name) + self.assertEqual(self.node.get_namespace(), server.node_namespace) + assert 'test_msgs/srv/Empty' in server.topic_type + if 'test_msgs/srv/Empty_Request' == server.topic_type: + actual_qos_profile = server.qos_profile + assert server.endpoint_type == TopicEndpointTypeEnum.SUBSCRIPTION + self.assert_qos_equal(qos_profile2, actual_qos_profile, is_publisher=False) + elif 'test_msgs/srv/Empty_Response' == server.topic_type: + actual_qos_profile = server.qos_profile + assert server.endpoint_type == TopicEndpointTypeEnum.PUBLISHER + self.assert_qos_equal(qos_profile2, actual_qos_profile, is_publisher=True) + + # 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): short_topic_name = 'chatter' fq_topic_name = '%s/%s' % (TEST_NAMESPACE, short_topic_name)