From 833bd3971c9719d4ff5f49b0f8c5ad77f2ada8e0 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sat, 2 Dec 2023 16:23:08 +0900 Subject: [PATCH] Implement methods for node info Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 121 ++++++++++++++---- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 8 ++ .../src/rmw_get_node_info_and_types.cpp | 90 +++++++++---- 3 files changed, 168 insertions(+), 51 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index ceaad89c..bcd48a76 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -27,8 +27,10 @@ #include "rcutils/logging_macros.h" #include "rcutils/strdup.h" -#include "rmw/sanity_checks.h" #include "rmw/error_handling.h" +#include "rmw/sanity_checks.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" #include "graph_cache.hpp" @@ -517,43 +519,33 @@ _demangle_if_ros_type(const std::string & dds_type_string) return type_namespace + type_name; } -} // namespace - -///============================================================================= -rmw_ret_t GraphCache::get_topic_names_and_types( +rmw_ret_t fill_names_and_types( + const GraphNode::TopicMap & entity_map, rcutils_allocator_t * allocator, - bool no_demangle, - rmw_names_and_types_t * topic_names_and_types) const + rmw_names_and_types_t * names_and_types) { - static_cast(no_demangle); - RCUTILS_CHECK_ALLOCATOR_WITH_MSG( - allocator, "get_node_names allocator is not valid", return RMW_RET_INVALID_ARGUMENT); - - std::lock_guard lock(graph_mutex_); - const size_t topic_number = graph_topics_.size(); - - rmw_ret_t ret = rmw_names_and_types_init(topic_names_and_types, topic_number, allocator); + const std::size_t entity_size = entity_map.size(); + rmw_ret_t ret = rmw_names_and_types_init(names_and_types, entity_size, allocator); if (ret != RMW_RET_OK) { return ret; } auto cleanup_names_and_types = rcpputils::make_scope_exit( - [topic_names_and_types] { - rmw_ret_t fail_ret = rmw_names_and_types_fini(topic_names_and_types); + [names_and_types] { + rmw_ret_t fail_ret = rmw_names_and_types_fini(names_and_types); if (fail_ret != RMW_RET_OK) { RMW_SAFE_FWRITE_TO_STDERR("failed to cleanup names and types during error handling"); } }); - // Fill topic names and types. std::size_t index = 0; - for (const std::pair & item : graph_topics_) { - topic_names_and_types->names.data[index] = rcutils_strdup(item.first.c_str(), *allocator); - if (!topic_names_and_types->names.data[index]) { + for (const std::pair & item : entity_map) { + names_and_types->names.data[index] = rcutils_strdup(item.first.c_str(), *allocator); + if (!names_and_types->names.data[index]) { return RMW_RET_BAD_ALLOC; } { rcutils_ret_t rcutils_ret = rcutils_string_array_init( - &topic_names_and_types->types[index], + &names_and_types->types[index], item.second.size(), allocator); if (RCUTILS_RET_OK != rcutils_ret) { @@ -568,7 +560,7 @@ rmw_ret_t GraphCache::get_topic_names_and_types( RMW_SET_ERROR_MSG("failed to allocate memory for type name"); return RMW_RET_BAD_ALLOC; } - topic_names_and_types->types[index].data[type_index] = type_name; + names_and_types->types[index].data[type_index] = type_name; ++type_index; } ++index; @@ -579,13 +571,29 @@ rmw_ret_t GraphCache::get_topic_names_and_types( return RMW_RET_OK; } +} // namespace + +///============================================================================= +rmw_ret_t GraphCache::get_topic_names_and_types( + rcutils_allocator_t * allocator, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) const +{ + static_cast(no_demangle); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "get_node_names allocator is not valid", return RMW_RET_INVALID_ARGUMENT); + + std::lock_guard lock(graph_mutex_); + return fill_names_and_types(graph_topics_, allocator, topic_names_and_types); +} + ///============================================================================= rmw_ret_t GraphCache::count_publishers( const char * topic_name, size_t * count) const { *count = 0; - + std::lock_guard lock(graph_mutex_); if (graph_topics_.count(topic_name) != 0) { for (const std::pair & it : graph_topics_.at(topic_name)) { // Iterate through all the types and increment count. @@ -602,7 +610,7 @@ rmw_ret_t GraphCache::count_subscriptions( size_t * count) const { *count = 0; - + std::lock_guard lock(graph_mutex_); if (graph_topics_.count(topic_name) != 0) { for (const std::pair & it : graph_topics_.at(topic_name)) { // Iterate through all the types and increment count. @@ -612,3 +620,66 @@ rmw_ret_t GraphCache::count_subscriptions( return RMW_RET_OK; } + +///============================================================================= +rmw_ret_t GraphCache::get_entity_names_and_types_by_node( + liveliness::EntityType entity_type, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * names_and_types) const +{ + static_cast(no_demangle); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT); + int validation_result = RMW_NODE_NAME_VALID; + rmw_ret_t ret = rmw_validate_node_name(node_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return ret; + } + if (RMW_NODE_NAME_VALID != validation_result) { + const char * reason = rmw_node_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("node_name argument is invalid: %s", reason); + return RMW_RET_INVALID_ARGUMENT; + } + validation_result = RMW_NAMESPACE_VALID; + ret = rmw_validate_namespace(node_namespace, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return ret; + } + if (RMW_NAMESPACE_VALID != validation_result) { + const char * reason = rmw_namespace_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("node_namespace argument is invalid: %s", reason); + return RMW_RET_INVALID_ARGUMENT; + } + ret = rmw_names_and_types_check_zero(names_and_types); + if (RMW_RET_OK != ret) { + return ret; + } + + std::lock_guard lock(graph_mutex_); + + // Check if namespace exists. + NamespaceMap::const_iterator ns_it = graph_.find(node_namespace); + if (ns_it == graph_.end()) { + return RMW_RET_OK; + } + + // Check if node exists. + NodeMap::const_iterator node_it = ns_it->second.find(node_name); + if (node_it == ns_it->second.end()) { + return RMW_RET_OK; + } + + // TODO(Yadunund): Support service and client when ready. + if (entity_type == EntityType::Publisher) { + return fill_names_and_types(node_it->second->pubs_, allocator, names_and_types); + } else if (entity_type == EntityType::Subscription) { + return fill_names_and_types(node_it->second->subs_, allocator, names_and_types); + } else { + return RMW_RET_OK; + } + + return RMW_RET_OK; +} diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index de78dd96..3d0005f2 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -100,6 +100,14 @@ class GraphCache final const char * topic_name, size_t * count) const; + rmw_ret_t get_entity_names_and_types_by_node( + liveliness::EntityType entity_type, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * names_and_types) const; + private: /* namespace_1: diff --git a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp index cd16fd6d..79e069ab 100644 --- a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "detail/identifier.hpp" +#include "detail/liveliness_utils.hpp" +#include "detail/rmw_data_types.hpp" #include "rmw/get_node_info_and_types.h" +#include "rmw/impl/cpp/macros.hpp" extern "C" { @@ -28,13 +32,21 @@ rmw_get_subscriber_names_and_types_by_node( bool no_demangle, rmw_names_and_types_t * topic_names_and_types) { - static_cast(node); - static_cast(allocator); - static_cast(node_name); - static_cast(node_namespace); - static_cast(no_demangle); - static_cast(topic_names_and_types); - return RMW_RET_UNSUPPORTED; + 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(node->context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); + return node->context->impl->graph_cache.get_entity_names_and_types_by_node( + liveliness::EntityType::Subscription, + allocator, + node_name, + node_namespace, + no_demangle, + topic_names_and_types); } ///============================================================================== @@ -48,13 +60,21 @@ rmw_get_publisher_names_and_types_by_node( bool no_demangle, rmw_names_and_types_t * topic_names_and_types) { - static_cast(node); - static_cast(allocator); - static_cast(node_name); - static_cast(node_namespace); - static_cast(no_demangle); - static_cast(topic_names_and_types); - return RMW_RET_UNSUPPORTED; + 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(node->context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); + return node->context->impl->graph_cache.get_entity_names_and_types_by_node( + liveliness::EntityType::Publisher, + allocator, + node_name, + node_namespace, + no_demangle, + topic_names_and_types); } ///============================================================================== @@ -67,12 +87,21 @@ rmw_get_service_names_and_types_by_node( const char * node_namespace, rmw_names_and_types_t * service_names_and_types) { - static_cast(node); - static_cast(allocator); - static_cast(node_name); - static_cast(node_namespace); - static_cast(service_names_and_types); - return RMW_RET_UNSUPPORTED; + 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(node->context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); + return node->context->impl->graph_cache.get_entity_names_and_types_by_node( + liveliness::EntityType::Service, + allocator, + node_name, + node_namespace, + false, + service_names_and_types); } ///============================================================================== @@ -85,11 +114,20 @@ rmw_get_client_names_and_types_by_node( const char * node_namespace, rmw_names_and_types_t * service_names_and_types) { - static_cast(node); - static_cast(allocator); - static_cast(node_name); - static_cast(node_namespace); - static_cast(service_names_and_types); - return RMW_RET_UNSUPPORTED; + 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(node->context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); + return node->context->impl->graph_cache.get_entity_names_and_types_by_node( + liveliness::EntityType::Client, + allocator, + node_name, + node_namespace, + false, + service_names_and_types); } } // extern "C"