Implement check for service availability

Signed-off-by: Yadunund <[email protected]>
ros2 · Jan 12, 2024 · 5f936c3 · 5f936c3
1 parent 2175a27
commit 5f936c3
Show file tree

Hide file tree

Showing 3 changed files with 56 additions and 6 deletions.
diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp
@@ -963,3 +963,23 @@ rmw_ret_t GraphCache::get_entities_info_by_topic(
   cleanup_endpoints_info.cancel();
   return RMW_RET_OK;
 }
+
+
+///=============================================================================
+rmw_ret_t GraphCache::service_server_is_available(
+  const char * service_name,
+  const char * service_type,
+  bool * is_available)
+{
+  *is_available = false;
+  std::lock_guard<std::mutex> lock(graph_mutex_);
+  GraphNode::TopicMap::iterator service_it = graph_services_.find(service_name);
+  if (service_it != graph_services_.end()){
+    GraphNode::TopicDataMap::iterator type_it = service_it->second.find(service_type);
+    if (type_it != service_it->second.end()){
+      *is_available = true;
+    }
+  }
+
+  return RMW_RET_OK;
+}
diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp
@@ -141,6 +141,11 @@ class GraphCache final
     bool no_demangle,
     rmw_topic_endpoint_info_array_t * endpoints_info) const;
 
+  rmw_ret_t service_server_is_available(
+    const char * service_name,
+    const char * service_type,
+    bool * is_available);
+
 private:
   /*
   namespace_1:

diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp
@@ -3300,12 +3300,37 @@ rmw_service_server_is_available(
   const rmw_client_t * client,
   bool * is_available)
 {
-  // TODO(francocipollone): Provide a proper implementation.
-  //                        We need graph cache information for this.
-  *is_available = true;
-  static_cast<void>(node);
-  static_cast<void>(client);
-  return RMW_RET_OK;
+  RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node,
+    node->implementation_identifier,
+    rmw_zenoh_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(client->data, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(is_available, RMW_RET_INVALID_ARGUMENT);
+
+  rmw_client_data_t * client_data = static_cast<rmw_client_data_t *>(client->data);
+  if (client_data == nullptr) {
+    RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Unable to retreive client_data from client for service %s", client->service_name);
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  std::string service_type = client_data->request_type_support->get_name();
+  size_t suffix_substring_position = service_type.find("Request_");
+  if (std::string::npos != suffix_substring_position) {
+    service_type = service_type.substr(0, suffix_substring_position);
+  }
+  else {
+    RCUTILS_LOG_ERROR_NAMED(
+      "rmw_zenoh_cpp",
+      "Unexpected type %s for client %s. Report this bug",
+      service_type.c_str(), client->service_name);
+      return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  return node->context->impl->graph_cache.service_server_is_available(
+    client->service_name, service_type.c_str(), is_available);
 }
 
 //==============================================================================