add clients & services count (ros2#2072)

* add clients & services count * add count clients,services tests Signed-off-by: leeminju531 <[email protected]>
Barry-Xu-2018 · Jan 12, 2024 · a749f65 · a749f65
1 parent 5b31083
commit a749f65
Show file tree

Hide file tree

Showing 11 changed files with 166 additions and 0 deletions.
diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
@@ -1315,6 +1315,26 @@ class Node : public std::enable_shared_from_this<Node>
   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
@@ -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
@@ -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
@@ -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
@@ -530,6 +530,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<rclcpp::TopicEndpointInfo>
 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
@@ -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
@@ -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
@@ -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
@@ -691,6 +691,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
@@ -395,6 +395,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<rclcpp::TopicEndpointInfo>
 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
@@ -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) {