Implement functions to get publisher and subcription informations lik…

…e QoS policies from topic name (#511)

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

rcl/include/rcl/graph.h

@@ -22,6 +22,7 @@ extern "C"
 
 #include <rmw/names_and_types.h>
 #include <rmw/get_topic_names_and_types.h>
+#include <rmw/topic_endpoint_info_array.h>
 
 #include "rcutils/types.h"
 
@@ -420,8 +421,8 @@ rcl_names_and_types_fini(rcl_names_and_types_t * names_and_types);
  *
  * \param[in] node the handle to the node being used to query the ROS graph
  * \param[in] allocator used to control allocation and deallocation of names
- * \param[out] node_names struct storing discovered node names.
- * \param[out] node_namesspaces struct storing discovered node namespaces.
+ * \param[out] node_names struct storing discovered node names
+ * \param[out] node_namesspaces struct storing discovered node namespaces
  * \return `RCL_RET_OK` if the query was successful, or
  * \return `RCL_RET_ERROR` if an unspecified error occurs.
  */
@@ -524,6 +525,126 @@ rcl_count_subscribers(
   const char * topic_name,
   size_t * count);
 
+/// Return a list of all publishers to a topic.
+/**
+ * The `node` parameter must point to a valid node.
+ *
+ * The `topic_name` parameter must not be `NULL`.
+ *
+ * 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.
+ * In either case, the topic name should always be fully qualified.
+ *
+ * Each element in the `publishers_info` array will contain the node name, node namespace,
+ * topic type, gid and the qos profile of the publisher.
+ * It is the responsibility of the caller to ensure that `publishers_info` parameter points
+ * to a valid struct of type rmw_topic_endpoint_info_array_t.
+ * The `count` field inside the struct must be set to 0 and the `info_array` field inside
+ * the struct must be set to null.
+ * \see rmw_get_zero_initialized_topic_endpoint_info_array
+ *
+ * The `allocator` will be used to allocate memory to the `info_array` member
+ * inside of `publishers_info`.
+ * Moreover, every const char * member inside of
+ * rmw_topic_endpoint_info_t will be assigned a copied value on allocated memory.
+ * \see rmw_topic_endpoint_info_set_node_name and the likes.
+ * However, it is the responsibility of the caller to
+ * reclaim any allocated resources to `publishers_info` to avoid leaking memory.
+ * \see rmw_topic_endpoint_info_array_fini
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Maybe [1]
+ * <i>[1] implementation may need to protect the data structure with a lock</i>
+ *
+ * \param[in] node the handle to the node being used to query the ROS graph
+ * \param[in] allocator allocator to be used when allocating space for
+ *            the array inside publishers_info
+ * \param[in] topic_name the name of the topic in question
+ * \param[in] no_mangle if true, the topic_name will be expanded to its fully qualified name
+ * \param[out] publishers_info a struct representing a list of publisher information
+ * \return `RCL_RET_OK` if the query was successful, or
+ * \return `RCL_RET_NODE_INVALID` if the node is invalid, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_get_publishers_info_by_topic(
+  const rcl_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * topic_name,
+  bool no_mangle,
+  rmw_topic_endpoint_info_array_t * publishers_info);
+
+/// Return a list of all subscriptions to a topic.
+/**
+ * The `node` parameter must point to a valid node.
+ *
+ * The `topic_name` parameter must not be `NULL`.
+ *
+ * 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.
+ * In either case, the topic name should always be fully qualified.
+ *
+ * Each element in the `subscriptions_info` array will contain the node name, node namespace,
+ * topic type, gid and the qos profile of the subscription.
+ * It is the responsibility of the caller to ensure that `subscriptions_info` parameter points
+ * to a valid struct of type rmw_topic_endpoint_info_array_t.
+ * The `count` field inside the struct must be set to 0 and the `info_array` field inside
+ * the struct must be set to null.
+ * \see rmw_get_zero_initialized_topic_endpoint_info_array
+ *
+ * The `allocator` will be used to allocate memory to the `info_array` member
+ * inside of `subscriptions_info`.
+ * Moreover, every const char * member inside of
+ * rmw_topic_endpoint_info_t will be assigned a copied value on allocated memory.
+ * \see rmw_topic_endpoint_info_set_node_name and the likes.
+ * However, it is the responsibility of the caller to
+ * reclaim any allocated resources to `subscriptions_info` to avoid leaking memory.
+ * \see rmw_topic_endpoint_info_array_fini
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Maybe [1]
+ * <i>[1] implementation may need to protect the data structure with a lock</i>
+ *
+ * \param[in] node the handle to the node being used to query the ROS graph
+ * \param[in] allocator allocator to be used when allocating space for
+ *            the array inside publishers_info
+ * \param[in] topic_name the name of the topic in question
+ * \param[in] no_mangle if true, the topic_name will be expanded to its fully qualified name
+ * \param[out] subscriptions_info a struct representing a list of subscriptions information
+ * \return `RCL_RET_OK` if the query was successful, or
+ * \return `RCL_RET_NODE_INVALID` if the node is invalid, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_get_subscriptions_info_by_topic(
+  const rcl_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * topic_name,
+  bool no_mangle,
+  rmw_topic_endpoint_info_array_t * subscriptions_info);
+
 /// Check if a service server is available for the given service client.
 /**
  * This function will return true for `is_available` if there is a service server

rcl/src/rcl/graph.c

@@ -25,9 +25,11 @@ extern "C"
 #include "rmw/error_handling.h"
 #include "rmw/get_node_info_and_types.h"
 #include "rmw/get_service_names_and_types.h"
+#include "rmw/get_topic_endpoint_info.h"
 #include "rmw/get_topic_names_and_types.h"
 #include "rmw/names_and_types.h"
 #include "rmw/rmw.h"
+#include "rmw/topic_endpoint_info_array.h"
 #include "rmw/validate_namespace.h"
 #include "rmw/validate_node_name.h"
 
@@ -375,6 +377,90 @@ rcl_count_subscribers(
   return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
 }
 
+typedef rmw_ret_t (* get_topic_endpoint_info_func_t)(
+  const rmw_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * topic_name,
+  bool no_mangle,
+  rmw_topic_endpoint_info_array_t * info_array);
+
+rcl_ret_t
+__rcl_get_info_by_topic(
+  const rcl_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * topic_name,
+  bool no_mangle,
+  rmw_topic_endpoint_info_array_t * info_array,
+  get_topic_endpoint_info_func_t get_topic_endpoint_info)
+{
+  if (!rcl_node_is_valid(node)) {
+    return RCL_RET_NODE_INVALID;  // error already set.
+  }
+  const rcl_node_options_t * node_options = rcl_node_get_options(node);
+  if (!node_options) {
+    return RCL_RET_NODE_INVALID;  // shouldn't happen, but error is already set if so
+  }
+  RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT);
+  rmw_error_string_t error_string;
+  rmw_ret_t rmw_ret = rmw_topic_endpoint_info_array_check_zero(info_array);
+  if (rmw_ret != RMW_RET_OK) {
+    error_string = rmw_get_error_string();
+    rmw_reset_error();
+    RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "rmw_topic_endpoint_info_array_t must be zero initialized: %s,\n"
+      "Use rmw_get_zero_initialized_topic_endpoint_info_array",
+      error_string.str);
+    return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+  }
+  rmw_ret = get_topic_endpoint_info(
+    rcl_node_get_rmw_handle(node),
+    allocator,
+    topic_name,
+    no_mangle,
+    info_array);
+  if (rmw_ret != RMW_RET_OK) {
+    error_string = rmw_get_error_string();
+    rmw_reset_error();
+    RCL_SET_ERROR_MSG(error_string.str);
+  }
+  return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+}
+
+rcl_ret_t
+rcl_get_publishers_info_by_topic(
+  const rcl_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * topic_name,
+  bool no_mangle,
+  rmw_topic_endpoint_info_array_t * publishers_info)
+{
+  return __rcl_get_info_by_topic(
+    node,
+    allocator,
+    topic_name,
+    no_mangle,
+    publishers_info,
+    rmw_get_publishers_info_by_topic);
+}
+
+rcl_ret_t
+rcl_get_subscriptions_info_by_topic(
+  const rcl_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * topic_name,
+  bool no_mangle,
+  rmw_topic_endpoint_info_array_t * subscriptions_info)
+{
+  return __rcl_get_info_by_topic(
+    node,
+    allocator,
+    topic_name,
+    no_mangle,
+    subscriptions_info,
+    rmw_get_subscriptions_info_by_topic);
+}
+
 rcl_ret_t
 rcl_service_server_is_available(
   const rcl_node_t * node,

rcl/test/CMakeLists.txt

@@ -101,6 +101,21 @@ function(test_target_function)
     ${AMENT_GTEST_ARGS}
   )
 
+  set(AMENT_GTEST_ARGS "")
+  # TODO(mm318): why rmw_connext tests run much slower than rmw_fastrtps and rmw_opensplice tests
+  if(rmw_implementation STREQUAL "rmw_connext_cpp")
+    message(STATUS "Increasing test_info_by_topic${target_suffix} test timeout.")
+    set(AMENT_GTEST_ARGS TIMEOUT 120)
+  endif()
+  rcl_add_custom_gtest(test_info_by_topic${target_suffix}
+    SRCS rcl/test_info_by_topic.cpp
+    ENV ${rmw_implementation_env_var}
+    APPEND_LIBRARY_DIRS ${extra_lib_dirs}
+    LIBRARIES ${PROJECT_NAME}
+    AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs"
+    ${AMENT_GTEST_ARGS}
+  )
+
   rcl_add_custom_gtest(test_count_matched${target_suffix}
     SRCS rcl/test_count_matched.cpp
     ENV ${rmw_implementation_env_var}

rcl/test/rcl/test_graph.cpp

@@ -916,8 +916,8 @@ class NodeGraphMultiNodeFixture : public CLASSNAME(TestGraphFixture, RMW_IMPLEME
   /**
    * Verify the number of subsystems each node should have.
    *
-   * @param node_state expected state of node
-   * @param remote_node_state expected state of remote node
+   * \param node_state expected state of node
+   * \param remote_node_state expected state of remote node
    */
   void VerifySubsystemCount(
     const expected_node_state && node_state,