add : get clients, servers info

Signed-off-by: Minju, Lee <[email protected]>

Author: leeminju531

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,

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,

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

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_

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)