add : get clients, servers info

leeminju531 · leeminju531 · commit da404d2e9771 · 2024-06-25T21:42:18.000+09:00
Signed-off-by: Minju, Lee &lt;dlalswn531@naver.com&gt;
diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
@@ -1405,6 +1405,58 @@ class Node : public std::enable_shared_from_this<Node>
   std::vector<rclcpp::TopicEndpointInfo>
   get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const;
 
+  /// Return the service endpoint information about clients on a given service.
+  /**
+   * The returned parameter is a list of service endpoint information, where each item will contain
+   * the node name, node namespace, service type, endpoint type, service endpoint's GID, and its QoS
+   * profile.
+   *
+   * When the `no_mangle` parameter is `true`, the provided `service_name` should be a valid service
+   * name for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps).
+   * When the `no_mangle` parameter is `false`, the provided `service_name` should follow
+   * ROS service name conventions.
+   *
+   * 'service_name` may be a relative, private, or fully qualified service name.
+   * A relative or private service will be expanded using this node's namespace and name.
+   * The queried `service_name` is not remapped.
+   *
+   * \param[in] service_name the actual service name used; it will not be automatically remapped.
+   * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware service name,
+   *   otherwise it should be a valid ROS service name. Defaults to `false`.
+   * \return a list of TopicEndpointInfo representing all the clients on this service.
+   * \throws InvalidTopicNameError if the given service_name is invalid.
+   * \throws std::runtime_error if internal error happens.
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_clients_info_by_service(const std::string & service_name, bool no_mangle = false) const;
+
+  /// Return the service endpoint information about servers on a given service.
+  /**
+   * The returned parameter is a list of service endpoint information, where each item will contain
+   * the node name, node namespace, service type, endpoint type, service endpoint's GID, and its QoS
+   * profile.
+   *
+   * When the `no_mangle` parameter is `true`, the provided `service_name` should be a valid service
+   * name for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps).
+   * When the `no_mangle` parameter is `false`, the provided `service_name` should follow
+   * ROS service name conventions.
+   *
+   * 'service_name` may be a relative, private, or fully qualified service name.
+   * A relative or private service will be expanded using this node's namespace and name.
+   * The queried `service_name` is not remapped.
+   *
+   * \param[in] service_name the actual service name used; it will not be automatically remapped.
+   * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware service name,
+   *   otherwise it should be a valid ROS service name. Defaults to `false`.
+   * \return a list of TopicEndpointInfo representing all the servers on this service.
+   * \throws InvalidTopicNameError if the given service_name is invalid.
+   * \throws std::runtime_error if internal error happens.
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_servers_info_by_service(const std::string & service_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
@@ -159,6 +159,18 @@ class NodeGraph : public NodeGraphInterface
     const std::string & topic_name,
     bool no_mangle = false) const override;
 
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_clients_info_by_service(
+    const std::string & service_name,
+    bool no_mangle = false) const override;
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_servers_info_by_service(
+    const std::string & service_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
@@ -408,6 +408,30 @@ class NodeGraphInterface
   virtual
   std::vector<rclcpp::TopicEndpointInfo>
   get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const = 0;
+
+  /// Return the service endpoint information about clients on a given service.
+  /**
+   * \param[in] service_name the actual service name used; it will not be automatically remapped.
+   * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware service name,
+   *   otherwise it should be a valid ROS service name.
+   * \sa rclcpp::Node::get_clients_info_by_service
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_clients_info_by_service(const std::string & service_name, bool no_mangle = false) const = 0;
+
+  /// Return the service endpoint information about servers on a given service.
+  /**
+   * \param[in] service_name the actual service name used; it will not be automatically remapped.
+   * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware service name,
+   *   otherwise it should be a valid ROS service name.
+   * \sa rclcpp::Node::get_servers_info_by_service
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_servers_info_by_service(const std::string & service_name, bool no_mangle = false) const = 0;
 };
 
 }  // namespace node_interfaces
diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp
@@ -547,6 +547,19 @@ Node::get_subscriptions_info_by_topic(const std::string & topic_name, bool no_ma
   return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle);
 }
 
+std::vector<rclcpp::TopicEndpointInfo>
+Node::get_clients_info_by_service(const std::string & service_name, bool no_mangle) const
+{
+  return node_graph_->get_clients_info_by_service(service_name, no_mangle);
+}
+
+std::vector<rclcpp::TopicEndpointInfo>
+Node::get_servers_info_by_service(const std::string & service_name, bool no_mangle) const
+{
+  return node_graph_->get_servers_info_by_service(service_name, no_mangle);
+}
+
+
 void
 Node::for_each_callback_group(
   const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func)
diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp
@@ -664,7 +664,8 @@ get_info_by_topic(
   rclcpp::node_interfaces::NodeBaseInterface * node_base,
   const std::string & topic_name,
   bool no_mangle,
-  FunctionT rcl_get_info_by_topic)
+  bool is_service,
+  FunctionT rcl_get_info_by_topic_or_service)
 {
   std::string fqdn;
   auto rcl_node_handle = node_base->get_rcl_node_handle();
@@ -676,7 +677,7 @@ get_info_by_topic(
       topic_name,
       rcl_node_get_name(rcl_node_handle),
       rcl_node_get_namespace(rcl_node_handle),
-      false);    // false = not a service
+      is_service);
 
     // Get the node options
     const rcl_node_options_t * node_options = rcl_node_get_options(rcl_node_handle);
@@ -687,9 +688,9 @@ get_info_by_topic(
     if (node_options->use_global_arguments) {
       global_args = &(rcl_node_handle->context->global_arguments);
     }
-
+    auto rcl_remap_name = is_service ? rcl_remap_service_name : rcl_remap_topic_name;
     char * remapped_topic_name = nullptr;
-    rcl_ret_t ret = rcl_remap_topic_name(
+    rcl_ret_t ret = rcl_remap_name(
       &(node_options->arguments),
       global_args,
       fqdn.c_str(),
@@ -708,7 +709,13 @@ get_info_by_topic(
   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);
+    rcl_get_info_by_topic_or_service(
+      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(":");
@@ -745,6 +752,7 @@ NodeGraph::get_publishers_info_by_topic(
     node_base_,
     topic_name,
     no_mangle,
+    false,
     rcl_get_publishers_info_by_topic);
 }
 
@@ -758,9 +766,38 @@ NodeGraph::get_subscriptions_info_by_topic(
     node_base_,
     topic_name,
     no_mangle,
+    false,
     rcl_get_subscriptions_info_by_topic);
 }
 
+static constexpr char kClientsEndpointTypeName[] = "clients";
+std::vector<rclcpp::TopicEndpointInfo>
+NodeGraph::get_clients_info_by_service(
+  const std::string & service_name,
+  bool no_mangle) const
+{
+  return get_info_by_topic<kClientsEndpointTypeName>(
+    node_base_,
+    service_name,
+    no_mangle,
+    true,
+    rcl_get_clients_info_by_service);
+}
+
+static constexpr char kServersEndpointTypeName[] = "servers";
+std::vector<rclcpp::TopicEndpointInfo>
+NodeGraph::get_servers_info_by_service(
+  const std::string & service_name,
+  bool no_mangle) const
+{
+  return get_info_by_topic<kServersEndpointTypeName>(
+    node_base_,
+    service_name,
+    no_mangle,
+    true,
+    rcl_get_servers_info_by_service);
+}
+
 std::string &
 rclcpp::TopicEndpointInfo::node_name()
 {
diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp
@@ -3263,6 +3263,135 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) {
   }, rclcpp::exceptions::InvalidTopicNameError);
 }
 
+// test that calling get_clients_info_by_service and get_servers_info_by_service
+TEST_F(TestNode, get_clients_servers_info_by_service) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  std::string service_name = "test_service_info";
+  std::string fq_service_name = rclcpp::expand_topic_or_service_name(
+    service_name, node->get_name(), node->get_namespace(), true);
+
+  // Lists should be empty
+  EXPECT_TRUE(node->get_clients_info_by_service(fq_service_name).empty());
+  EXPECT_TRUE(node->get_servers_info_by_service(fq_service_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 client = node->create_client<test_msgs::srv::Empty>(service_name, qos);
+  // Wait for the underlying RMW implementation to catch up with graph changes
+  auto client_is_generated =
+    [&]() {return node->get_clients_info_by_service(fq_service_name).size() > 0u;};
+  ASSERT_TRUE(wait_for_event(node, client_is_generated));
+  // List should have at least one item
+  auto client_list = node->get_clients_info_by_service(fq_service_name);
+  ASSERT_GE(client_list.size(), (size_t)1);
+  // Server list should be empty
+  EXPECT_TRUE(node->get_servers_info_by_service(fq_service_name).empty());
+  // Verify client list has the right data.
+  for(auto client_info : client_list) {
+    EXPECT_EQ(node->get_name(), client_info.node_name());
+    EXPECT_EQ(node->get_namespace(), client_info.node_namespace());
+    ASSERT_NE(client_info.topic_type().find("test_msgs/srv/Empty"), std::string::npos);
+    if(client_info.topic_type() == "test_msgs/srv/Empty_Request") {
+      EXPECT_EQ(rclcpp::EndpointType::Publisher, client_info.endpoint_type());
+      auto actual_qos_profile = client_info.qos_profile().get_rmw_qos_profile();
+      {
+        SCOPED_TRACE("Publisher QOS 1");
+        expect_qos_profile_eq(qos.get_rmw_qos_profile(), actual_qos_profile, true);
+      }
+    } else if(client_info.topic_type() == "test_msgs/srv/Empty_Response") {
+      EXPECT_EQ(rclcpp::EndpointType::Subscription, client_info.endpoint_type());
+      auto actual_qos_profile = client_info.qos_profile().get_rmw_qos_profile();
+      {
+        SCOPED_TRACE("Publisher QOS 1");
+        expect_qos_profile_eq(qos.get_rmw_qos_profile(), actual_qos_profile, false);
+      }
+    }
+  }
+
+  // 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_AUTOMATIC,
+    {5, 23456},
+    false
+  };
+  rclcpp::QoS qos2 = rclcpp::QoS(qos_initialization2, rmw_qos_profile_default2);
+  auto callback = [](test_msgs::srv::Empty_Request::ConstSharedPtr req,
+    test_msgs::srv::Empty_Response::ConstSharedPtr resp) {
+      (void)req;
+      (void)resp;
+    };
+  auto server = node->create_service<test_msgs::srv::Empty>(service_name, callback, qos2);
+  // Wait for the underlying RMW implementation to catch up with graph changes
+  auto server_is_generated =
+    [&]() {return node->get_servers_info_by_service(fq_service_name).size() > 0u;};
+  ASSERT_TRUE(wait_for_event(node, server_is_generated));
+  // Both lists should have at least one item
+  client_list = node->get_clients_info_by_service(fq_service_name);
+  auto server_list = node->get_servers_info_by_service(fq_service_name);
+  ASSERT_GE(client_list.size(), (size_t)1);
+  ASSERT_GE(server_list.size(), (size_t)1);
+  // Verify server list has the right data.
+  for(auto server_info : server_list) {
+    EXPECT_EQ(node->get_name(), server_info.node_name());
+    EXPECT_EQ(node->get_namespace(), server_info.node_namespace());
+    ASSERT_NE(server_info.topic_type().find("test_msgs/srv/Empty"), std::string::npos);
+    if(server_info.topic_type() == "test_msgs/srv/Empty_Request") {
+      EXPECT_EQ(rclcpp::EndpointType::Subscription, server_info.endpoint_type());
+      auto actual_qos_profile = server_info.qos_profile().get_rmw_qos_profile();
+      {
+        SCOPED_TRACE("Publisher QOS 1");
+        expect_qos_profile_eq(qos2.get_rmw_qos_profile(), actual_qos_profile, false);
+      }
+    } else if(server_info.topic_type() == "test_msgs/srv/Empty_Response") {
+      EXPECT_EQ(rclcpp::EndpointType::Publisher, server_info.endpoint_type());
+      auto actual_qos_profile = server_info.qos_profile().get_rmw_qos_profile();
+      {
+        SCOPED_TRACE("Publisher QOS 1");
+        expect_qos_profile_eq(qos2.get_rmw_qos_profile(), actual_qos_profile, true);
+      }
+    }
+  }
+
+  // Error cases
+  EXPECT_THROW(
+  {
+    client_list = node->get_clients_info_by_service("13");
+  }, rclcpp::exceptions::InvalidServiceNameError);
+  EXPECT_THROW(
+  {
+    server_list = node->get_servers_info_by_service("13");
+  }, rclcpp::exceptions::InvalidServiceNameError);
+}
+
 TEST_F(TestNode, callback_groups) {
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
   size_t num_callback_groups_in_basic_node = 0;
diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp
@@ -722,6 +722,22 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
   std::vector<rclcpp::TopicEndpointInfo>
   get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const;
 
+  /// Return the service endpoint information about clients on a given service.
+  /**
+   * \sa rclcpp::Node::get_clients_info_by_service
+   */
+  RCLCPP_LIFECYCLE_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_clients_info_by_service(const std::string & service_name, bool no_mangle = false) const;
+
+  /// Return the service endpoint information about server on a given service.
+  /**
+   * \sa rclcpp::Node::get_servers_info_by_service
+   */
+  RCLCPP_LIFECYCLE_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_servers_info_by_service(const std::string & service_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
@@ -410,6 +410,18 @@ LifecycleNode::get_subscriptions_info_by_topic(const std::string & topic_name, b
   return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle);
 }
 
+std::vector<rclcpp::TopicEndpointInfo>
+LifecycleNode::get_clients_info_by_service(const std::string & service_name, bool no_mangle) const
+{
+  return node_graph_->get_clients_info_by_service(service_name, no_mangle);
+}
+
+std::vector<rclcpp::TopicEndpointInfo>
+LifecycleNode::get_servers_info_by_service(const std::string & service_name, bool no_mangle) const
+{
+  return node_graph_->get_servers_info_by_service(service_name, no_mangle);
+}
+
 void
 LifecycleNode::for_each_callback_group(
   const rclcpp::node_interfaces::NodeBaseInterface::CallbackGroupFunction & func)
