- Added and implemented get_publishers_info_by_topic and get_subscrip…

…tions_info_by

topic in node.py
- Added and implemented helper method in common.h and common.c to
convert rmw_topic_info_array_t to a python list of dictionaries.
- Added and implemented rclpy_get_publishers_info_by_topic and
rclpy_get_subscriptions_information_by_topic in _rclpy.c
- Added tests for node.get_publishers_info_by_topic and
node.get_subscriptions_info_by_topic

Signed-off-by: Jaison Titus <jaisontj92@gmail.com>

rclpy/rclpy/node.py

@@ -1637,3 +1637,65 @@ 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[Dict]:
+        fq_topic_name = expand_topic_name(topic_name, self.get_name(), self.get_namespace())
+        validate_topic_name(fq_topic_name)
+        with self.handle as node_capsule:
+            return func(node_capsule, fq_topic_name, no_mangle)
+
+    def get_publishers_info_by_topic(
+            self, topic_name: str, no_mangle: bool = False) -> List[Dict]:
+        """
+        Return a list of publishers publishing to a given topic.
+
+        The returned parameter is a list of dictionaries, where each dictionary will
+        contain the node name, node namespace, topic type, participant's GID and its QoS profile.
+
+        '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, if
+        the 'no_mangle' param is set to false.
+
+        The queried topic name is not remapped.
+
+        'no_mangle' defaults to false.
+
+        :param topic_name: the topic_name on which to find the publishers.
+        :param no_mangle: if false, the given topic name will be expanded
+                          to its fully qualified name.
+        :return: a list of dictionaries representing 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[Dict]:
+        """
+        Return a list of subscriptions to a given topic.
+
+        The returned parameter is a list of dictionaries, where each dictionary will contain
+        the node name, node namespace, topic type, participant's GID and its QoS profile.
+
+        '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, if
+        the 'no_mangle' param is set to false.
+
+        The queried topic name is not remapped.
+
+        'no_mangle' defaults to false.
+
+        :param topic_name: the topic_name on which to find the subscriptions.
+        :param no_mangle: if false, the given topic name will be expanded
+                          to its fully qualified name.
+        :return: a list of dictionaries representing all the subscriptions on this topic.
+        """
+        return self._get_info_by_topic(
+            topic_name,
+            no_mangle,
+            _rclpy.rclpy_get_subscriptions_info_by_topic)

rclpy/src/rclpy/_rclpy.c

@@ -31,6 +31,7 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 #include <rmw/serialized_message.h>
+#include <rmw/topic_info_array.h>
 #include <rmw/types.h>
 #include <rmw/validate_full_topic_name.h>
 #include <rmw/validate_namespace.h>
@@ -851,6 +852,91 @@ 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)(
+  const rcl_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * topic_name,
+  bool no_mangle,
+  rmw_topic_info_array_t * info_array);
+
+static PyObject *
+_get_info_by_topic(
+  PyObject * args, const char * type,
+  rcl_get_info_by_topic_func rcl_get_info_by_topic)
+{
+  PyObject * pynode;
+  const char * topic_name;
+  PyObject * pyno_mangle;
+
+  if (!PyArg_ParseTuple(args, "OsO", &pynode, &topic_name, &pyno_mangle)) {
+    return NULL;
+  }
+
+  rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t");
+  if (!node) {
+    return NULL;
+  }
+  bool no_mangle = PyObject_IsTrue(pyno_mangle);
+  rcutils_allocator_t allocator = rcutils_get_default_allocator();
+  rmw_topic_info_array_t info_array = rmw_get_zero_initialized_topic_info_array();
+  rcl_ret_t ret = rcl_get_info_by_topic(node, &allocator, topic_name, no_mangle, &info_array);
+  rmw_ret_t fini_ret;
+  if (ret != RCL_RET_OK) {
+    PyErr_Format(PyExc_RuntimeError, "Failed to get information by topic for %s: %s",
+      type, rcl_get_error_string().str);
+    rcl_reset_error();
+    fini_ret = rmw_topic_info_array_fini(&allocator, &info_array);
+    if (fini_ret != RMW_RET_OK) {
+      PyErr_Format(PyExc_RuntimeError, "rmw_topic_info_array_fini failed.");
+      rmw_reset_error();
+    }
+    return NULL;
+  }
+  PyObject * py_info_array = rclpy_convert_to_py_topic_info_list(&info_array);
+  fini_ret = rmw_topic_info_array_fini(&allocator, &info_array);
+  if (fini_ret != RMW_RET_OK) {
+    PyErr_Format(PyExc_RuntimeError, "rmw_topic_info_array_fini failed.");
+    rmw_reset_error();
+    return NULL;
+  }
+  return py_info_array;
+}
+
+/// Returns a list of publishers, publishing to a 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 the given topic name will be
+ *            expanded to its fully qualified 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);
+}
+
+/// Returns a list of subscriptions to a 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 the given topic name will be
+ *            expanded to its fully qualified 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.
@@ -4750,6 +4836,14 @@ 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 a list of publishers for a topic."
+  },
+  {
+    "rclpy_get_subscriptions_info_by_topic", rclpy_get_subscriptions_info_by_topic, METH_VARARGS,
+    "Get a list of subscriptions for a topic."
+  },
   {
     "rclpy_expand_topic_name", rclpy_expand_topic_name, METH_VARARGS,
     "Expand a topic name."

rclpy/src/rclpy_common/include/rclpy_common/common.h

@@ -123,4 +123,13 @@ RCLPY_COMMON_PUBLIC
 PyObject *
 rclpy_convert_to_py(void * message, PyObject * pyclass);
 
+/// Convert a C rmw_topic_info_array_t into a Python list.
+/**
+ * \param[in] info_array a pointer to a rmw_topic_info_array_t
+ * \return Python list
+ */
+RCLPY_COMMON_PUBLIC
+PyObject *
+rclpy_convert_to_py_topic_info_list(const rmw_topic_info_array_t * info_array);
+
 #endif  // RCLPY_COMMON__COMMON_H_

rclpy/src/rclpy_common/src/common.c

@@ -338,3 +338,96 @@ rclpy_convert_to_py(void * message, PyObject * pyclass)
   }
   return convert(message);
 }
+
+PyObject *
+rclpy_convert_to_py_topic_info_list(const rmw_topic_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_info_t topic_info = info_array->info_array[i];
+    const rmw_qos_profile_t * qos_profile = topic_info.qos_profile;
+    PyObject * py_qos_profile = rclpy_common_convert_to_qos_dict(qos_profile);
+    if (!py_qos_profile) {
+      Py_DECREF(py_info_array);
+      return NULL;
+    }
+    const char * node_name = topic_info.node_name;
+    PyObject * py_node_name = PyUnicode_FromString(node_name);
+    if (!py_node_name) {
+      Py_DECREF(py_info_array);
+      Py_DECREF(py_qos_profile);
+      return NULL;
+    }
+
+    const char * node_namespace = topic_info.node_namespace;
+    PyObject * py_node_namespace = PyUnicode_FromString(node_namespace);
+    if (!py_node_namespace) {
+      Py_DECREF(py_info_array);
+      Py_DECREF(py_qos_profile);
+      Py_DECREF(py_node_name);
+      return NULL;
+    }
+
+    const char * topic_type = topic_info.topic_type;
+    PyObject * py_topic_type = PyUnicode_FromString(topic_type);
+    if (!py_topic_type) {
+      Py_DECREF(py_info_array);
+      Py_DECREF(py_qos_profile);
+      Py_DECREF(py_node_name);
+      Py_DECREF(py_node_namespace);
+      return NULL;
+    }
+
+    const char * gid = topic_info.gid;
+    PyObject * py_gid = PyUnicode_FromString(gid);
+    if (!py_gid) {
+      Py_DECREF(py_info_array);
+      Py_DECREF(py_qos_profile);
+      Py_DECREF(py_node_name);
+      Py_DECREF(py_node_namespace);
+      Py_DECREF(py_topic_type);
+      return NULL;
+    }
+
+    // Create dictionary that represents rmw_topic_info_t
+    PyObject * py_topic_info_dict = PyDict_New();
+    if (!py_topic_info_dict) {
+      Py_DECREF(py_info_array);
+      Py_DECREF(py_qos_profile);
+      Py_DECREF(py_node_name);
+      Py_DECREF(py_node_namespace);
+      Py_DECREF(py_topic_type);
+      Py_DECREF(py_gid);
+      return NULL;
+    }
+    // Populate keyword arguments
+    // A success returns 0, and a failure returns -1
+    int set_result = 0;
+    set_result += PyDict_SetItemString(py_topic_info_dict, "qos_profile", py_qos_profile);
+    set_result += PyDict_SetItemString(py_topic_info_dict, "node_name", py_node_name);
+    set_result += PyDict_SetItemString(py_topic_info_dict, "node_namespace", py_node_namespace);
+    set_result += PyDict_SetItemString(py_topic_info_dict, "topic_type", py_topic_type);
+    set_result += PyDict_SetItemString(py_topic_info_dict, "gid", py_gid);
+    if (set_result != 0) {
+      Py_DECREF(py_info_array);
+      Py_DECREF(py_qos_profile);
+      Py_DECREF(py_node_name);
+      Py_DECREF(py_node_namespace);
+      Py_DECREF(py_topic_type);
+      Py_DECREF(py_gid);
+      Py_DECREF(py_topic_info_dict);
+      return NULL;
+    }
+    // add this dict to the list
+    PyList_SET_ITEM(py_info_array, i, py_topic_info_dict);
+  }
+  return py_info_array;
+}