Implement functions to get publisher and subcription informations like QoS policies from topic name

rclcpp/CMakeLists.txt

@@ -247,6 +247,7 @@ if(BUILD_TESTING)
       "rmw"
       "rosidl_generator_cpp"
       "rosidl_typesupport_cpp"
+      "test_msgs"
     )
     target_link_libraries(test_node ${PROJECT_NAME})
   endif()

rclcpp/include/rclcpp/node.hpp

@@ -867,6 +867,58 @@ class Node : public std::enable_shared_from_this<Node>
   size_t
   count_subscribers(const std::string & topic_name) const;
 
+  /// Return the topic endpoint information about publishers on a given topic.
+  /**
+   * The returned parameter is a list of topic endpoint information, where each item will contain
+   * the node name, node namespace, topic type, endpoint type, topic endpoint's GID, and its QoS
+   * profile.
+   *
+   * 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.
+   *
+   * `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.
+   * The queried `topic_name` is not remapped.
+   *
+   * \param[in] topic_name the topic_name on which to find the publishers.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name. Defaults to `false`.
+   * \return a list of TopicEndpointInfo representing all the publishers on this topic.
+   * \throws InvalidTopicNameError if the given topic_name is invalid.
+   * \throws std::runtime_error if internal error happens.
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle = false) const;
+
+  /// Return the topic endpoint information about subscriptions on a given topic.
+  /**
+   * The returned parameter is a list of topic endpoint information, where each item will contain
+   * the node name, node namespace, topic type, endpoint type, topic endpoint's GID, and its QoS
+   * profile.
+   *
+   * 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.
+   *
+   * `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.
+   * The queried `topic_name` is not remapped.
+   *
+   * \param[in] topic_name the topic_name on which to find the subscriptions.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name. Defaults to `false`.
+   * \return a list of TopicEndpointInfo representing all the subscriptions on this topic.
+   * \throws InvalidTopicNameError if the given topic_name is invalid.
+   * \throws std::runtime_error if internal error happens.
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const;
+
   /// Return a graph event, which will be set anytime a graph change occurs.
   /* The graph Event object is a loan which must be returned.
    * The Event object is scoped and therefore to return the loan just let it go

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -30,6 +30,7 @@
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rmw/topic_endpoint_info_array.h"
 
 namespace rclcpp
 {
@@ -117,6 +118,18 @@ class NodeGraph : public NodeGraphInterface
   size_t
   count_graph_users() override;
 
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_publishers_info_by_topic(
+    const std::string & topic_name,
+    bool no_mangle = false) const override;
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_subscriptions_info_by_topic(
+    const std::string & topic_name,
+    bool no_mangle = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeGraph)
 

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -15,20 +15,57 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
 
+#include <algorithm>
+#include <array>
 #include <chrono>
 #include <map>
 #include <string>
 #include <utility>
 #include <vector>
 
+#include "rcl/graph.h"
 #include "rcl/guard_condition.h"
 
 #include "rclcpp/event.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
+
+enum class EndpointType
+{
+  Invalid = RMW_ENDPOINT_INVALID,
+  Publisher = RMW_ENDPOINT_PUBLISHER,
+  Subscription = RMW_ENDPOINT_SUBSCRIPTION
+};
+
+/**
+ * Struct that contains topic endpoint information like the associated node name, node namespace,
+ * topic type, endpoint type, endpoint GID, and its QoS.
+ */
+struct RCLCPP_PUBLIC TopicEndpointInfo
+{
+  std::string node_name;
+  std::string node_namespace;
+  std::string topic_type;
+  rclcpp::EndpointType endpoint_type;
+  std::array<uint8_t, RMW_GID_STORAGE_SIZE> endpoint_gid;
+  rclcpp::QoS qos;
+
+  /// Constructor which converts rcl_topic_endpoint_info_t to TopicEndpointInfo.
+  explicit TopicEndpointInfo(const rcl_topic_endpoint_info_t & info)
+  : node_name(info.node_name),
+    node_namespace(info.node_namespace),
+    topic_type(info.topic_type),
+    endpoint_type(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
+    qos({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile)
+  {
+    std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid.begin());
+  }
+};
+
 namespace node_interfaces
 {
 
@@ -150,6 +187,24 @@ class NodeGraphInterface
   virtual
   size_t
   count_graph_users() = 0;
+
+  /// Return the topic endpoint information about publishers on a given topic.
+  /**
+   * \sa rclcpp::Node::get_publishers_info_by_topic
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle = false) const = 0;
+
+  /// Return the topic endpoint information about subscriptions on a given topic.
+  /**
+   * \sa rclcpp::Node::get_subscriptions_info_by_topic
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/src/rclcpp/node.cpp

@@ -364,6 +364,18 @@ Node::count_subscribers(const std::string & topic_name) const
   return node_graph_->count_subscribers(topic_name);
 }
 
+std::vector<rclcpp::TopicEndpointInfo>
+Node::get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle) const
+{
+  return node_graph_->get_publishers_info_by_topic(topic_name, no_mangle);
+}
+
+std::vector<rclcpp::TopicEndpointInfo>
+Node::get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle) const
+{
+  return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle);
+}
+
 const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
 Node::get_callback_groups() const
 {

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -21,8 +21,9 @@
 #include <vector>
 
 #include "rcl/graph.h"
-#include "rclcpp/exceptions.hpp"
+#include "rcl/remap.h"
 #include "rclcpp/event.hpp"
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/graph_listener.hpp"
 
 using rclcpp::node_interfaces::NodeGraph;
@@ -369,3 +370,117 @@ NodeGraph::count_graph_users()
 {
   return graph_users_count_.load();
 }
+
+static
+std::vector<rclcpp::TopicEndpointInfo>
+convert_to_topic_info_list(const rcl_topic_endpoint_info_array_t & info_array)
+{
+  std::vector<rclcpp::TopicEndpointInfo> topic_info_list;
+  for (size_t i = 0; i < info_array.count; ++i) {
+    topic_info_list.push_back(rclcpp::TopicEndpointInfo(info_array.info_array[i]));
+  }
+  return topic_info_list;
+}
+
+template<const char * EndpointType, typename FunctionT>
+static std::vector<rclcpp::TopicEndpointInfo>
+get_info_by_topic(
+  rclcpp::node_interfaces::NodeBaseInterface * node_base,
+  const std::string & topic_name,
+  bool no_mangle,
+  FunctionT rcl_get_info_by_topic)
+{
+  std::string fqdn;
+  auto rcl_node_handle = node_base->get_rcl_node_handle();
+
+  if (no_mangle) {
+    fqdn = topic_name;
+  } else {
+    fqdn = rclcpp::expand_topic_or_service_name(
+      topic_name,
+      rcl_node_get_name(rcl_node_handle),
+      rcl_node_get_namespace(rcl_node_handle),
+      false);    // false = not a service
+
+    // Get the node options
+    const rcl_node_options_t * node_options = rcl_node_get_options(rcl_node_handle);
+    if (nullptr == node_options) {
+      throw std::runtime_error("Need valid node options in get_info_by_topic()");
+    }
+    const rcl_arguments_t * global_args = nullptr;
+    if (node_options->use_global_arguments) {
+      global_args = &(rcl_node_handle->context->global_arguments);
+    }
+
+    char * remapped_topic_name = nullptr;
+    rcl_ret_t ret = rcl_remap_topic_name(
+      &(node_options->arguments),
+      global_args,
+      fqdn.c_str(),
+      rcl_node_get_name(rcl_node_handle),
+      rcl_node_get_namespace(rcl_node_handle),
+      node_options->allocator,
+      &remapped_topic_name);
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, std::string("Failed to remap topic name ") + fqdn);
+    } else if (nullptr != remapped_topic_name) {
+      fqdn = remapped_topic_name;
+      node_options->allocator.deallocate(remapped_topic_name, node_options->allocator.state);
+    }
+  }
+
+  rcutils_allocator_t allocator = rcutils_get_default_allocator();
+  rcl_topic_endpoint_info_array_t info_array = rcl_get_zero_initialized_topic_endpoint_info_array();
+  rcl_ret_t ret =
+    rcl_get_info_by_topic(rcl_node_handle, &allocator, fqdn.c_str(), no_mangle, &info_array);
+  if (RCL_RET_OK != ret) {
+    auto error_msg =
+      std::string("Failed to get information by topic for ") + EndpointType + std::string(":");
+    if (RCL_RET_UNSUPPORTED == ret) {
+      error_msg += std::string("function not supported by RMW_IMPLEMENTATION");
+    } else {
+      error_msg += rcl_get_error_string().str;
+    }
+    rcl_reset_error();
+    if (RCL_RET_OK != rcl_topic_endpoint_info_array_fini(&info_array, &allocator)) {
+      error_msg += std::string(", failed also to cleanup topic info array, leaking memory: ") +
+        rcl_get_error_string().str;
+      rcl_reset_error();
+    }
+    throw_from_rcl_error(ret, error_msg);
+  }
+
+  std::vector<rclcpp::TopicEndpointInfo> topic_info_list = convert_to_topic_info_list(info_array);
+  ret = rcl_topic_endpoint_info_array_fini(&info_array, &allocator);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "rcl_topic_info_array_fini failed.");
+  }
+
+  return topic_info_list;
+}
+
+static const char kPublisherEndpointTypeName[] = "publishers";
+std::vector<rclcpp::TopicEndpointInfo>
+NodeGraph::get_publishers_info_by_topic(
+  const std::string & topic_name,
+  bool no_mangle) const
+{
+  return get_info_by_topic<kPublisherEndpointTypeName>(
+    node_base_,
+    topic_name,
+    no_mangle,
+    rcl_get_publishers_info_by_topic);
+}
+
+static const char kSubscriptionEndpointTypeName[] = "subscriptions";
+std::vector<rclcpp::TopicEndpointInfo>
+NodeGraph::get_subscriptions_info_by_topic(
+  const std::string & topic_name,
+  bool no_mangle) const
+{
+  return get_info_by_topic<kSubscriptionEndpointTypeName>(
+    node_base_,
+    topic_name,
+    no_mangle,
+    rcl_get_subscriptions_info_by_topic);
+}