From 6f080b9f6d3fe1f7dc72a400e0754299603d8688 Mon Sep 17 00:00:00 2001
From: Jacob Perron <jacob@openrobotics.org>
Date: Wed, 12 Jun 2019 12:48:45 -0700
Subject: [PATCH] Add function for getting clients by node

Signed-off-by: Jacob Perron <jacob@openrobotics.org>
---
 .../src/rmw_node_info_and_types.cpp           | 48 ++++++++++++++++++-
 rmw_opensplice_cpp/src/types.cpp              | 12 ++++-
 rmw_opensplice_cpp/src/types.hpp              |  3 +-
 3 files changed, 59 insertions(+), 4 deletions(-)

diff --git a/rmw_opensplice_cpp/src/rmw_node_info_and_types.cpp b/rmw_opensplice_cpp/src/rmw_node_info_and_types.cpp
index ac5c208d..e3e2423f 100644
--- a/rmw_opensplice_cpp/src/rmw_node_info_and_types.cpp
+++ b/rmw_opensplice_cpp/src/rmw_node_info_and_types.cpp
@@ -264,9 +264,53 @@ rmw_get_service_names_and_types_by_node(
     return get_guid_err;
   }
 
-  // combine publisher and subscriber information
   std::map<std::string, std::set<std::string>> services;
-  node_info->subscriber_listener->fill_service_names_and_types_by_participant(services, key);
+  node_info->subscriber_listener->fill_service_names_and_types_by_participant(
+    services,
+    key,
+    "Request");
+
+  rmw_ret_t rmw_ret;
+  rmw_ret = copy_services_to_names_and_types(services, allocator, service_names_and_types);
+  if (rmw_ret != RMW_RET_OK) {
+    return rmw_ret;
+  }
+  return RMW_RET_OK;
+}
+
+rmw_ret_t
+rmw_get_client_names_and_types_by_node(
+  const rmw_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * node_name,
+  const char * node_namespace,
+  rmw_names_and_types_t * service_names_and_types)
+{
+  rmw_ret_t ret = validate_node(node, allocator);
+  if (ret != RMW_RET_OK) {
+    return ret;
+  }
+  ret = rmw_names_and_types_check_zero(service_names_and_types);
+  if (ret != RMW_RET_OK) {
+    return ret;
+  }
+  ret = validate_names_and_namespace(node_name, node_namespace);
+  if (ret != RMW_RET_OK) {
+    return ret;
+  }
+
+  auto node_info = static_cast<OpenSpliceStaticNodeInfo *>(node->data);
+  DDS::InstanceHandle_t key;
+  auto get_guid_err = __get_key(node_info, node_name, node_namespace, key);
+  if (get_guid_err != RMW_RET_OK) {
+    return get_guid_err;
+  }
+
+  std::map<std::string, std::set<std::string>> services;
+  node_info->subscriber_listener->fill_service_names_and_types_by_participant(
+    services,
+    key,
+    "Reply");
 
   rmw_ret_t rmw_ret;
   rmw_ret = copy_services_to_names_and_types(services, allocator, service_names_and_types);
diff --git a/rmw_opensplice_cpp/src/types.cpp b/rmw_opensplice_cpp/src/types.cpp
index c64ded75..06ee76bd 100644
--- a/rmw_opensplice_cpp/src/types.cpp
+++ b/rmw_opensplice_cpp/src/types.cpp
@@ -139,7 +139,8 @@ void CustomDataReaderListener::fill_topic_names_and_types_by_participant(
 
 void CustomDataReaderListener::fill_service_names_and_types_by_participant(
   std::map<std::string, std::set<std::string>> & services,
-  DDS::InstanceHandle_t & participant)
+  DDS::InstanceHandle_t & participant,
+  const std::string & suffix)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   const auto & map = topic_cache.getTopicTypesByGuid(participant);
@@ -155,6 +156,15 @@ void CustomDataReaderListener::fill_service_names_and_types_by_participant(
       // not a service
       continue;
     }
+    // Check if the topic suffix matches and is at the end of the name
+    const std::string & topic_name = it.first;
+    auto suffix_position = topic_name.rfind(suffix);
+    if (suffix_position == std::string::npos ||
+      topic_name.length() - suffix_position - suffix.length() != 0)
+    {
+      continue;
+    }
+
     for (auto & itt : it.second) {
       std::string service_type = _demangle_service_type_only(itt);
       if (service_type.length()) {
diff --git a/rmw_opensplice_cpp/src/types.hpp b/rmw_opensplice_cpp/src/types.hpp
index 62168042..125194a6 100644
--- a/rmw_opensplice_cpp/src/types.hpp
+++ b/rmw_opensplice_cpp/src/types.hpp
@@ -97,7 +97,8 @@ class CustomDataReaderListener
 
   void fill_service_names_and_types_by_participant(
     std::map<std::string, std::set<std::string>> & services,
-    DDS::InstanceHandle_t & participant);
+    DDS::InstanceHandle_t & participant,
+    const std::string & suffix);
 
   size_t count_topic(const char * topic_name);