From 2d9c6ea3a77103b91f7984269d163037961a4c04 Mon Sep 17 00:00:00 2001 From: Barry Xu <43591679+Barry-Xu-2018@users.noreply.github.com> Date: Sat, 15 Feb 2020 04:25:03 +0800 Subject: [PATCH] Implement functions to get publisher and subcription informations like QoS policies from topic name (#960) Signed-off-by: Barry Xu Signed-off-by: Miaofei Co-authored-by: Miaofei Mei --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/node.hpp | 52 +++++ .../rclcpp/node_interfaces/node_graph.hpp | 13 ++ .../node_interfaces/node_graph_interface.hpp | 118 +++++++++++ rclcpp/src/rclcpp/node.cpp | 12 ++ .../src/rclcpp/node_interfaces/node_graph.cpp | 190 +++++++++++++++++- rclcpp/test/test_node.cpp | 117 +++++++++++ .../rclcpp_lifecycle/lifecycle_node.hpp | 16 ++ rclcpp_lifecycle/src/lifecycle_node.cpp | 12 ++ 9 files changed, 530 insertions(+), 1 deletion(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 42f9eb3f81..dc8f5d6bf9 100644 --- a/rclcpp/CMakeLists.txt +++ b/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() diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 4b6bcfbc91..b9398f567c 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -867,6 +867,58 @@ class Node : public std::enable_shared_from_this 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 + 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 + 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 diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index ff2d91b261..26d91aecfd 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/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 + get_publishers_info_by_topic( + const std::string & topic_name, + bool no_mangle = false) const override; + + RCLCPP_PUBLIC + std::vector + get_subscriptions_info_by_topic( + const std::string & topic_name, + bool no_mangle = false) const override; + private: RCLCPP_DISABLE_COPY(NodeGraph) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 022984dd47..26ab3118ef 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -15,20 +15,120 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ +#include +#include #include #include #include #include #include +#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. + */ +class TopicEndpointInfo +{ +public: + /// Construct a TopicEndpointInfo from a rcl_topic_endpoint_info_t. + RCLCPP_PUBLIC + 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(info.endpoint_type)), + qos_profile_({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()); + } + + /// Get a mutable reference to the node name. + RCLCPP_PUBLIC + std::string & + node_name(); + + /// Get a const reference to the node name. + RCLCPP_PUBLIC + const std::string & + node_name() const; + + /// Get a mutable reference to the node namespace. + RCLCPP_PUBLIC + std::string & + node_namespace(); + + /// Get a const reference to the node namespace. + RCLCPP_PUBLIC + const std::string & + node_namespace() const; + + /// Get a mutable reference to the topic type string. + RCLCPP_PUBLIC + std::string & + topic_type(); + + /// Get a const reference to the topic type string. + RCLCPP_PUBLIC + const std::string & + topic_type() const; + + /// Get a mutable reference to the topic endpoint type. + RCLCPP_PUBLIC + rclcpp::EndpointType & + endpoint_type(); + + /// Get a const reference to the topic endpoint type. + RCLCPP_PUBLIC + const rclcpp::EndpointType & + endpoint_type() const; + + /// Get a mutable reference to the GID of the topic endpoint. + RCLCPP_PUBLIC + std::array & + endpoint_gid(); + + /// Get a const reference to the GID of the topic endpoint. + RCLCPP_PUBLIC + const std::array & + endpoint_gid() const; + + /// Get a mutable reference to the QoS profile of the topic endpoint. + RCLCPP_PUBLIC + rclcpp::QoS & + qos_profile(); + + /// Get a const reference to the QoS profile of the topic endpoint. + RCLCPP_PUBLIC + const rclcpp::QoS & + qos_profile() const; + +private: + std::string node_name_; + std::string node_namespace_; + std::string topic_type_; + rclcpp::EndpointType endpoint_type_; + std::array endpoint_gid_; + rclcpp::QoS qos_profile_; +}; + namespace node_interfaces { @@ -150,6 +250,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 + 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 + get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const = 0; }; } // namespace node_interfaces diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 3cc13b0947..a2bf2707d2 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/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 +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 +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 & Node::get_callback_groups() const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 11347881b3..8df1c4bdbb 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -21,9 +21,11 @@ #include #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" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" using rclcpp::node_interfaces::NodeGraph; using rclcpp::exceptions::throw_from_rcl_error; @@ -369,3 +371,189 @@ NodeGraph::count_graph_users() { return graph_users_count_.load(); } + +static +std::vector +convert_to_topic_info_list(const rcl_topic_endpoint_info_array_t & info_array) +{ + std::vector 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 +static std::vector +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 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 +NodeGraph::get_publishers_info_by_topic( + const std::string & topic_name, + bool no_mangle) const +{ + return get_info_by_topic( + node_base_, + topic_name, + no_mangle, + rcl_get_publishers_info_by_topic); +} + +static const char kSubscriptionEndpointTypeName[] = "subscriptions"; +std::vector +NodeGraph::get_subscriptions_info_by_topic( + const std::string & topic_name, + bool no_mangle) const +{ + return get_info_by_topic( + node_base_, + topic_name, + no_mangle, + rcl_get_subscriptions_info_by_topic); +} + +std::string & +rclcpp::TopicEndpointInfo::node_name() +{ + return node_name_; +} + +const std::string & +rclcpp::TopicEndpointInfo::node_name() const +{ + return node_name_; +} + +std::string & +rclcpp::TopicEndpointInfo::node_namespace() +{ + return node_namespace_; +} + +const std::string & +rclcpp::TopicEndpointInfo::node_namespace() const +{ + return node_namespace_; +} + +std::string & +rclcpp::TopicEndpointInfo::topic_type() +{ + return topic_type_; +} + +const std::string & +rclcpp::TopicEndpointInfo::topic_type() const +{ + return topic_type_; +} + +rclcpp::EndpointType & +rclcpp::TopicEndpointInfo::endpoint_type() +{ + return endpoint_type_; +} + +const rclcpp::EndpointType & +rclcpp::TopicEndpointInfo::endpoint_type() const +{ + return endpoint_type_; +} + +std::array & +rclcpp::TopicEndpointInfo::endpoint_gid() +{ + return endpoint_gid_; +} + +const std::array & +rclcpp::TopicEndpointInfo::endpoint_gid() const +{ + return endpoint_gid_; +} + +rclcpp::QoS & +rclcpp::TopicEndpointInfo::qos_profile() +{ + return qos_profile_; +} + +const rclcpp::QoS & +rclcpp::TopicEndpointInfo::qos_profile() const +{ + return qos_profile_; +} diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp index 2bab00e678..003b3f6e12 100644 --- a/rclcpp/test/test_node.cpp +++ b/rclcpp/test/test_node.cpp @@ -26,6 +26,7 @@ #include "rclcpp/rclcpp.hpp" #include "rcpputils/filesystem_helper.hpp" +#include "test_msgs/msg/basic_types.hpp" class TestNode : public ::testing::Test { @@ -2510,3 +2511,119 @@ TEST_F(TestNode, set_on_parameters_set_callback_set_on_parameters_set_callback) node->set_parameter(rclcpp::Parameter("intparam", 40)); }, rclcpp::exceptions::ParameterModifiedInCallbackException); } + +// test that calling get_publishers_info_by_topic and get_subscriptions_info_by_topic +TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) { + auto node = std::make_shared("my_node", "/ns"); + std::string topic_name = "test_topic_info"; + std::string fq_topic_name = rclcpp::expand_topic_or_service_name( + topic_name, node->get_name(), node->get_namespace()); + + // Lists should be empty + EXPECT_TRUE(node->get_publishers_info_by_topic(fq_topic_name).empty()); + EXPECT_TRUE(node->get_subscriptions_info_by_topic(fq_topic_name).empty()); + + // Add a publisher + rclcpp::QoSInitialization qos_initialization = + { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 10 + }; + rmw_qos_profile_t rmw_qos_profile_default = + { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 10, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + {1, 12345}, + {20, 9887665}, + RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, + {5, 23456}, + false + }; + rclcpp::QoS qos = rclcpp::QoS(qos_initialization, rmw_qos_profile_default); + auto publisher = node->create_publisher(topic_name, qos); + // List should have one item + auto publisher_list = node->get_publishers_info_by_topic(fq_topic_name); + EXPECT_EQ(publisher_list.size(), (size_t)1); + // Subscription list should be empty + EXPECT_TRUE(node->get_subscriptions_info_by_topic(fq_topic_name).empty()); + // Verify publisher list has the right data. + EXPECT_EQ(node->get_name(), publisher_list[0].node_name()); + EXPECT_EQ(node->get_namespace(), publisher_list[0].node_namespace()); + EXPECT_EQ("test_msgs/msg/BasicTypes", publisher_list[0].topic_type()); + EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_list[0].endpoint_type()); + auto actual_qos_profile = publisher_list[0].qos_profile().get_rmw_qos_profile(); + + auto assert_qos_profile = [](const rmw_qos_profile_t & qos1, const rmw_qos_profile_t & qos2) { + // Depth and history are skipped because they are not retrieved. + EXPECT_EQ(qos1.reliability, qos2.reliability); + EXPECT_EQ(qos1.durability, qos2.durability); + EXPECT_EQ(memcmp(&qos1.deadline, &qos2.deadline, sizeof(struct rmw_time_t)), 0); + EXPECT_EQ(memcmp(&qos1.lifespan, &qos2.lifespan, sizeof(struct rmw_time_t)), 0); + EXPECT_EQ(qos1.liveliness, qos2.liveliness); + EXPECT_EQ( + memcmp( + &qos1.liveliness_lease_duration, + &qos2.liveliness_lease_duration, + sizeof(struct rmw_time_t)), + 0); + }; + + assert_qos_profile(qos.get_rmw_qos_profile(), actual_qos_profile); + + // Add a subscription + rclcpp::QoSInitialization qos_initialization2 = + { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 0 + }; + rmw_qos_profile_t rmw_qos_profile_default2 = + { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 0, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + {15, 1678}, + {29, 2345}, + RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE, + {5, 23456}, + false + }; + rclcpp::QoS qos2 = rclcpp::QoS(qos_initialization2, rmw_qos_profile_default2); + auto callback = [](const test_msgs::msg::BasicTypes::SharedPtr msg) { + (void)msg; + }; + auto subscriber = + node->create_subscription(topic_name, qos2, callback); + + // Both lists should have one item + publisher_list = node->get_publishers_info_by_topic(fq_topic_name); + auto subscription_list = node->get_subscriptions_info_by_topic(fq_topic_name); + EXPECT_EQ(publisher_list.size(), (size_t)1); + EXPECT_EQ(subscription_list.size(), (size_t)1); + + // Verify publisher and subscription list has the right data. + EXPECT_EQ(node->get_name(), publisher_list[0].node_name()); + EXPECT_EQ(node->get_namespace(), publisher_list[0].node_namespace()); + EXPECT_EQ("test_msgs/msg/BasicTypes", publisher_list[0].topic_type()); + EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_list[0].endpoint_type()); + auto publisher_qos_profile = publisher_list[0].qos_profile().get_rmw_qos_profile(); + assert_qos_profile(qos.get_rmw_qos_profile(), publisher_qos_profile); + EXPECT_EQ(node->get_name(), subscription_list[0].node_name()); + EXPECT_EQ(node->get_namespace(), subscription_list[0].node_namespace()); + EXPECT_EQ("test_msgs/msg/BasicTypes", subscription_list[0].topic_type()); + EXPECT_EQ(rclcpp::EndpointType::Subscription, subscription_list[0].endpoint_type()); + auto subscription_qos_profile = subscription_list[0].qos_profile().get_rmw_qos_profile(); + assert_qos_profile(qos2.get_rmw_qos_profile(), subscription_qos_profile); + + // Error cases + EXPECT_THROW( + { + publisher_list = node->get_publishers_info_by_topic("13"); + }, rclcpp::exceptions::InvalidTopicNameError); + EXPECT_THROW( + { + subscription_list = node->get_subscriptions_info_by_topic("13"); + }, rclcpp::exceptions::InvalidTopicNameError); +} diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 6ed96db535..6b6c213fd5 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -439,6 +439,22 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, size_t count_subscribers(const std::string & topic_name) const; + /// Return the topic endpoint information about publishers on a given topic. + /** + * \sa rclcpp::Node::get_publishers_info_by_topic + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + 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. + /** + * \sa rclcpp::Node::get_subscriptions_info_by_topic + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + 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 load just let it go diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index f75d6c2e44..973ec04270 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -290,6 +290,18 @@ LifecycleNode::count_subscribers(const std::string & topic_name) const return node_graph_->count_subscribers(topic_name); } +std::vector +LifecycleNode::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 +LifecycleNode::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 & LifecycleNode::get_callback_groups() const {