diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index c1ad85b417..6521264b86 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -1314,6 +1314,26 @@ class Node : public std::enable_shared_from_this size_t count_subscribers(const std::string & topic_name) const; + /// Return the number of clients created for a given service. + /** + * \param[in] service_name the actual service name used; it will not be automatically remapped. + * \return number of clients that have been created for the given service. + * \throws std::runtime_error if clients could not be counted + */ + RCLCPP_PUBLIC + size_t + count_clients(const std::string & service_name) const; + + /// Return the number of services created for a given service. + /** + * \param[in] service_name the actual service name used; it will not be automatically remapped. + * \return number of services that have been created for the given service. + * \throws std::runtime_error if services could not be counted + */ + RCLCPP_PUBLIC + size_t + count_services(const std::string & service_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 diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index d167515784..863dbee1bf 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -113,6 +113,14 @@ class NodeGraph : public NodeGraphInterface size_t count_subscribers(const std::string & topic_name) const override; + RCLCPP_PUBLIC + size_t + count_clients(const std::string & service_name) const override; + + RCLCPP_PUBLIC + size_t + count_services(const std::string & service_name) const override; + RCLCPP_PUBLIC const rcl_guard_condition_t * get_graph_guard_condition() const override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 5967997ac7..80abc308c1 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -305,6 +305,24 @@ class NodeGraphInterface size_t count_subscribers(const std::string & topic_name) const = 0; + /// Return the number of clients created for a given service. + /* + * \param[in] service_name the actual service name used; it will not be automatically remapped. + */ + RCLCPP_PUBLIC + virtual + size_t + count_clients(const std::string & service_name) const = 0; + + /// Return the number of services created for a given service. + /* + * \param[in] service_name the actual service name used; it will not be automatically remapped. + */ + RCLCPP_PUBLIC + virtual + size_t + count_services(const std::string & service_name) const = 0; + /// Return the rcl guard condition which is triggered when the ROS graph changes. RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 1d5b9113b5..50af3f1a89 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -96,6 +96,9 @@ * - Get the number of publishers or subscribers on a topic: * - rclcpp::Node::count_publishers() * - rclcpp::Node::count_subscribers() + * - Get the number of clients or servers on a service: + * - rclcpp::Node::count_clients() + * - rclcpp::Node::count_services() * * And components related to logging: * diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index c31f4ee1f0..c31903f2fe 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -522,6 +522,18 @@ Node::count_subscribers(const std::string & topic_name) const return node_graph_->count_subscribers(topic_name); } +size_t +Node::count_clients(const std::string & service_name) const +{ + return node_graph_->count_clients(service_name); +} + +size_t +Node::count_services(const std::string & service_name) const +{ + return node_graph_->count_services(service_name); +} + std::vector Node::get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle) const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 1228703cb6..f6e9e1fda0 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -498,6 +498,50 @@ NodeGraph::count_subscribers(const std::string & topic_name) const return count; } +size_t +NodeGraph::count_clients(const std::string & service_name) const +{ + auto rcl_node_handle = node_base_->get_rcl_node_handle(); + + auto fqdn = rclcpp::expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + + size_t count; + auto ret = rcl_count_clients(rcl_node_handle, fqdn.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count clients: ") + rmw_get_error_string().str); + // *INDENT-ON* + } + return count; +} + +size_t +NodeGraph::count_services(const std::string & service_name) const +{ + auto rcl_node_handle = node_base_->get_rcl_node_handle(); + + auto fqdn = rclcpp::expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + + size_t count; + auto ret = rcl_count_services(rcl_node_handle, fqdn.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count services: ") + rmw_get_error_string().str); + // *INDENT-ON* + } + return count; +} + const rcl_guard_condition_t * NodeGraph::get_graph_guard_condition() const { diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 70ccb5622d..3882d2b71c 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -129,6 +129,9 @@ TEST_F(TestNodeGraph, construct_from_node) EXPECT_EQ(0u, node_graph()->count_publishers("not_a_topic")); EXPECT_EQ(0u, node_graph()->count_subscribers("not_a_topic")); + EXPECT_EQ(0u, node_graph()->count_clients("not_a_service")); + EXPECT_EQ(0u, node_graph()->count_services("not_a_service")); + EXPECT_NE(nullptr, node_graph()->get_graph_guard_condition()); // get_graph_event is non-const @@ -534,6 +537,22 @@ TEST_F(TestNodeGraph, count_subscribers_rcl_error) std::runtime_error("could not count subscribers: error not set")); } +TEST_F(TestNodeGraph, count_clients_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_clients, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_clients("service"), + std::runtime_error("could not count clients: error not set")); +} + +TEST_F(TestNodeGraph, count_services_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_services, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_services("service"), + std::runtime_error("could not count services: error not set")); +} + TEST_F(TestNodeGraph, notify_shutdown) { EXPECT_NO_THROW(node()->get_node_graph_interface()->notify_shutdown()); diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 2f74a998c3..ad73aadc2a 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -3311,6 +3311,9 @@ TEST_F(TestNode, get_entity_names) { const auto service_names_and_types = node->get_service_names_and_types(); EXPECT_EQ(service_names_and_types.end(), service_names_and_types.find("service")); + EXPECT_EQ(0u, node->count_clients("service")); + EXPECT_EQ(0u, node->count_services("service")); + const auto service_names_and_types_by_node = node->get_service_names_and_types_by_node("node", "/ns"); EXPECT_EQ( diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 097537b53a..657ddddc34 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -690,6 +690,22 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, size_t count_subscribers(const std::string & topic_name) const; + /// Return the number of clients created for a given service. + /** + * \sa rclcpp::Node::count_clients + */ + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_clients(const std::string & service_name) const; + + /// Return the number of services created for a given service. + /** + * \sa rclcpp::Node::count_services + */ + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_services(const std::string & service_name) const; + /// Return the topic endpoint information about publishers on a given topic. /** * \sa rclcpp::Node::get_publishers_info_by_topic diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index a85d661e63..0c448cf5e6 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -386,6 +386,18 @@ LifecycleNode::count_subscribers(const std::string & topic_name) const return node_graph_->count_subscribers(topic_name); } +size_t +LifecycleNode::count_clients(const std::string & service_name) const +{ + return node_graph_->count_clients(service_name); +} + +size_t +LifecycleNode::count_services(const std::string & service_name) const +{ + return node_graph_->count_services(service_name); +} + std::vector LifecycleNode::get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle) const { diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index ddcc926527..fdb9be6153 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -726,6 +726,17 @@ TEST_F(TestDefaultStateMachine, test_graph_services) { EXPECT_STREQ( service_names_and_types["/testnode/get_transition_graph"][0].c_str(), "lifecycle_msgs/srv/GetAvailableTransitions"); + + EXPECT_EQ(0u, test_node->count_clients("/testnode/change_state")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_available_states")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_available_transitions")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_state")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_transition_graph")); + EXPECT_EQ(1u, test_node->count_services("/testnode/change_state")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_available_states")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_available_transitions")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_state")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_transition_graph")); } TEST_F(TestDefaultStateMachine, test_graph_services_by_node) {