From 1f3800713daadbda2dcfddf535234bc318891ec7 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 20 Feb 2024 11:57:14 +0800 Subject: [PATCH 01/12] Store current session id in graph cache Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 7 +++++ rmw_zenoh_cpp/src/detail/graph_cache.hpp | 7 +++++ rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 23 +++++++-------- rmw_zenoh_cpp/src/detail/liveliness_utils.hpp | 7 +++++ rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 2 +- .../src/detail/zenoh_router_check.cpp | 28 +------------------ .../src/rmw_get_node_info_and_types.cpp | 8 +++--- .../src/rmw_get_service_names_and_types.cpp | 2 +- .../src/rmw_get_topic_endpoint_info.cpp | 4 +-- .../src/rmw_get_topic_names_and_types.cpp | 2 +- rmw_zenoh_cpp/src/rmw_init.cpp | 19 +++++++------ rmw_zenoh_cpp/src/rmw_zenoh.cpp | 18 ++++++------ 12 files changed, 63 insertions(+), 64 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index b35c1309..cd2c02bb 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -56,6 +56,13 @@ TopicData::TopicData( stats_(std::move(stats)) {} +///============================================================================= +GraphCache::GraphCache(const z_id_t & zid) +: zid_str(liveliness::zid_to_str(zid)) +{ + // Do nothing. +} + ///============================================================================= void GraphCache::parse_put(const std::string & keyexpr) { diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 603422ac..69d80cd4 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -91,6 +91,12 @@ using GraphNodePtr = std::shared_ptr; class GraphCache final { public: + /// @brief Constructor + /// @param id The id of the zenoh session that is building the graph cache. + /// This is used to infer which entities originated from the current session + /// so that appropriate event callbacks may be triggered. + explicit GraphCache(const z_id_t & zid); + // Parse a PUT message over a token's key-expression and update the graph. void parse_put(const std::string & keyexpr); // Parse a DELETE message over a token's key-expression and update the graph. @@ -156,6 +162,7 @@ class GraphCache final bool * is_available); private: + std::string zid_str; /* namespace_1: node_1: diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index 03ffccfe..f8c2daa8 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -113,17 +113,6 @@ static const std::unordered_map str_to {std::to_string(RMW_QOS_POLICY_DURABILITY_UNKNOWN), RMW_QOS_POLICY_DURABILITY_UNKNOWN} }; -std::string zid_to_str(z_id_t id) -{ - std::stringstream ss; - ss << std::hex; - size_t i = 0; - for (; i < (sizeof(id.id)); i++) { - ss << static_cast(id.id[i]); - } - return ss.str(); -} - std::vector split_keyexpr( const std::string & keyexpr, const char delim = '/') @@ -208,6 +197,18 @@ std::optional keyexpr_to_qos(const std::string & keyexpr) return qos; } +///============================================================================= +std::string zid_to_str(const z_id_t & id) +{ + std::stringstream ss; + ss << std::hex; + size_t i = 0; + for (; i < (sizeof(id.id)); i++) { + ss << static_cast(id.id[i]); + } + return ss.str(); +} + ///============================================================================= std::string subscription_token(size_t domain_id) { diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp index 89b606dc..a0b1fa80 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp @@ -119,12 +119,15 @@ class Entity std::string keyexpr_; }; +///============================================================================= /// Replace "/" instances with "%". std::string mangle_name(const std::string & input); +///============================================================================= /// Replace "%" instances with "/". std::string demangle_name(const std::string & input); +///============================================================================= /** * Convert a rmw_qos_profile_t to a string with format: * @@ -140,9 +143,13 @@ std::string demangle_name(const std::string & input); */ std::string qos_to_keyexpr(rmw_qos_profile_t qos); +///============================================================================= /// Convert a rmw_qos_profile_t from a keyexpr. Return std::nullopt if invalid. std::optional keyexpr_to_qos(const std::string & keyexpr); +///============================================================================= +/// Convert a Zenoh id to a string. +std::string zid_to_str(const z_id_t & id); } // namespace liveliness diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index d81902b9..2a0c18f6 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -59,7 +59,7 @@ struct rmw_context_impl_s /// Guard condition that should be triggered when the graph changes. rmw_guard_condition_t * graph_guard_condition; - GraphCache graph_cache; + std::unique_ptr graph_cache; }; ///============================================================================= diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp index 353de39a..5e674089 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp @@ -21,32 +21,6 @@ #include #include -namespace -{ - -// Convert a Zenoh Id to a string -// Zenoh IDs are LSB-first 128bit unsigned and non-zero integers in hexadecimal lowercase. -// @param pid Zenoh Id to convert -std::string zid_to_str(const z_id_t & pid) -{ - std::stringstream ss; - int len = 0; - for (size_t i = 0; i < sizeof(pid.id); ++i) { - if (pid.id[i]) { - len = static_cast(i) + 1; - } - } - if (!len) { - ss << ""; - } else { - for (int i = len - 1; i >= 0; --i) { - ss << std::hex << std::setfill('0') << std::setw(2) << static_cast(pid.id[i]); - } - } - return ss.str(); -} - -} // namespace rmw_ret_t zenoh_router_check(z_session_t session) { @@ -55,9 +29,9 @@ rmw_ret_t zenoh_router_check(z_session_t session) // Define callback auto callback = [](const struct z_id_t * id, void * ctx) { - const std::string id_str = zid_to_str(*id); // Note: Callback is guaranteed to never be called // concurrently according to z_info_routers_zid docstring + static_cast(id); (*(static_cast(ctx)))++; }; 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 abcb6058..bac35e45 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 @@ -43,7 +43,7 @@ rmw_get_subscriber_names_and_types_by_node( 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( + return node->context->impl->graph_cache->get_entity_names_and_types_by_node( liveliness::EntityType::Subscription, allocator, node_name, @@ -71,7 +71,7 @@ rmw_get_publisher_names_and_types_by_node( 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( + return node->context->impl->graph_cache->get_entity_names_and_types_by_node( liveliness::EntityType::Publisher, allocator, node_name, @@ -98,7 +98,7 @@ rmw_get_service_names_and_types_by_node( 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( + return node->context->impl->graph_cache->get_entity_names_and_types_by_node( liveliness::EntityType::Service, allocator, node_name, @@ -125,7 +125,7 @@ rmw_get_client_names_and_types_by_node( 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( + return node->context->impl->graph_cache->get_entity_names_and_types_by_node( liveliness::EntityType::Client, allocator, node_name, diff --git a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp index 34f747f5..acda0b19 100644 --- a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp @@ -36,7 +36,7 @@ rmw_get_service_names_and_types( RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache.get_service_names_and_types( + return node->context->impl->graph_cache->get_service_names_and_types( allocator, service_names_and_types); } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp index d6f3b690..9dcc0d65 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -43,7 +43,7 @@ rmw_get_publishers_info_by_topic( 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_entities_info_by_topic( + return node->context->impl->graph_cache->get_entities_info_by_topic( liveliness::EntityType::Publisher, allocator, topic_name, @@ -69,7 +69,7 @@ rmw_get_subscriptions_info_by_topic( 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_entities_info_by_topic( + return node->context->impl->graph_cache->get_entities_info_by_topic( liveliness::EntityType::Subscription, allocator, topic_name, diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp index 7e2f3d24..673f56d5 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp @@ -37,7 +37,7 @@ rmw_get_topic_names_and_types( RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache.get_topic_names_and_types( + return node->context->impl->graph_cache->get_topic_names_and_types( allocator, no_demangle, topic_names_and_types); } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 5e0310a4..399b6b60 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -66,10 +66,10 @@ static void graph_sub_data_handler( switch (sample->kind) { case z_sample_kind_t::Z_SAMPLE_KIND_PUT: - context_impl->graph_cache.parse_put(keystr._cstr); + context_impl->graph_cache->parse_put(keystr._cstr); break; case z_sample_kind_t::Z_SAMPLE_KIND_DELETE: - context_impl->graph_cache.parse_del(keystr._cstr); + context_impl->graph_cache->parse_del(keystr._cstr); break; default: break; @@ -171,6 +171,10 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) z_close(z_move(context->impl->session)); }); + /// Initialize the graph cache. + z_id_t zid = z_info_zid(z_loan(context->impl->session)); + context->impl->graph_cache = std::make_unique(zid); + // Verify if the zenoh router is running. if ((ret = zenoh_router_check(z_loan(context->impl->session))) != RMW_RET_OK) { RMW_SET_ERROR_MSG("Error while checking for Zenoh router"); @@ -181,13 +185,12 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) if (shm_enabled._cstr != nullptr && strcmp(shm_enabled._cstr, "true") == 0) { - z_id_t id = z_info_zid(z_loan(context->impl->session)); - char idstr[sizeof(id.id) * 2 + 1]; // 2 bytes for each byte of the id, plus the trailing \0 + char idstr[sizeof(zid.id) * 2 + 1]; // 2 bytes for each byte of the id, plus the trailing \0 static constexpr size_t max_size_of_each = 3; // 2 for each byte, plus the trailing \0 - for (size_t i = 0; i < sizeof(id.id); ++i) { - snprintf(idstr + 2 * i, max_size_of_each, "%02x", id.id[i]); + for (size_t i = 0; i < sizeof(zid.id); ++i) { + snprintf(idstr + 2 * i, max_size_of_each, "%02x", zid.id[i]); } - idstr[sizeof(id.id) * 2] = '\0'; + idstr[sizeof(zid.id) * 2] = '\0'; // TODO(yadunund): Can we get the size of the shm from the config even though it's not // a standard parameter? context->impl->shm_manager = @@ -279,7 +282,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) if (z_reply_is_ok(&reply)) { z_sample_t sample = z_reply_ok(&reply); z_owned_str_t keystr = z_keyexpr_to_string(sample.keyexpr); - context->impl->graph_cache.parse_put(z_loan(keystr)); + context->impl->graph_cache->parse_put(z_loan(keystr)); z_drop(z_move(keystr)); } else { printf("[discovery] Received an error\n"); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 68b5ba37..917a1c02 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -913,7 +913,7 @@ rmw_publisher_count_matched_subscriptions( rmw_context_impl_t * context_impl = static_cast(pub_data->context->impl); RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); - return context_impl->graph_cache.publisher_count_matched_subscriptions( + return context_impl->graph_cache->publisher_count_matched_subscriptions( publisher, subscription_count); } @@ -1463,7 +1463,7 @@ rmw_subscription_count_matched_publishers( rmw_context_impl_t * context_impl = static_cast(sub_data->context->impl); RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); - return context_impl->graph_cache.subscription_count_matched_publishers( + return context_impl->graph_cache->subscription_count_matched_publishers( subscription, publisher_count); } @@ -3432,7 +3432,7 @@ rmw_get_node_names( rcutils_allocator_t * allocator = &node->context->options.allocator; RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache.get_node_names( + return node->context->impl->graph_cache->get_node_names( node_names, node_namespaces, nullptr, allocator); } @@ -3455,7 +3455,7 @@ rmw_get_node_names_with_enclaves( rcutils_allocator_t * allocator = &node->context->options.allocator; RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache.get_node_names( + return node->context->impl->graph_cache->get_node_names( node_names, node_namespaces, enclaves, allocator); } @@ -3486,7 +3486,7 @@ rmw_count_publishers( } RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache.count_publishers(topic_name, count); + return node->context->impl->graph_cache->count_publishers(topic_name, count); } //============================================================================== @@ -3516,7 +3516,7 @@ rmw_count_subscribers( } RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache.count_subscriptions(topic_name, count); + return node->context->impl->graph_cache->count_subscriptions(topic_name, count); } //============================================================================== @@ -3546,7 +3546,7 @@ rmw_count_clients( } RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache.count_clients(service_name, count); + return node->context->impl->graph_cache->count_clients(service_name, count); } //============================================================================== @@ -3576,7 +3576,7 @@ rmw_count_services( } RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache.count_services(service_name, count); + return node->context->impl->graph_cache->count_services(service_name, count); } //============================================================================== @@ -3661,7 +3661,7 @@ rmw_service_server_is_available( return RMW_RET_INVALID_ARGUMENT; } - return node->context->impl->graph_cache.service_server_is_available( + return node->context->impl->graph_cache->service_server_is_available( client->service_name, service_type.c_str(), is_available); } From 70a6c688bd33067f9db9e516520413315bbbe6e5 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 20 Feb 2024 14:14:46 +0800 Subject: [PATCH 02/12] Allow event callback functions to be registered with graph cache Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 25 ++++++++++++++++++++++- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 26 +++++++++++++++++++++++- 2 files changed, 49 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index cd2c02bb..94dcc847 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -58,7 +58,7 @@ TopicData::TopicData( ///============================================================================= GraphCache::GraphCache(const z_id_t & zid) -: zid_str(liveliness::zid_to_str(zid)) +: zid_str_(std::move(liveliness::zid_to_str(zid))) { // Do nothing. } @@ -1036,3 +1036,26 @@ rmw_ret_t GraphCache::service_server_is_available( return RMW_RET_OK; } + +///============================================================================= +void GraphCache::set_qos_event_callback( + const liveliness::Entity & entity, + const rmw_zenoh_event_type_t & event_type, + GraphCacheEventCallback callback) +{ + std::lock_guard lock(graph_mutex_); + + if (event_type > ZENOH_EVENT_ID_MAX) { + return; + } + + const std::string entity_key = entity.keyexpr(); + const GraphEventCallbackMap::iterator event_cb_it = event_callbacks_.find(entity_key); + if (event_cb_it == event_callbacks_.end()) { + // First time a callback is being set for this entity. + event_callbacks_[entity_key] = {}; + event_callbacks_[entity_key].insert(std::make_pair(event_type, std::move(callback))); + return; + } + event_cb_it->second.insert(std::make_pair(event_type, std::move(callback))); +} diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 69d80cd4..7a85bbbd 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -15,6 +15,7 @@ #ifndef DETAIL__GRAPH_CACHE_HPP_ #define DETAIL__GRAPH_CACHE_HPP_ +#include #include #include #include @@ -161,8 +162,19 @@ class GraphCache final const char * service_type, bool * is_available); + /// @brief Signature for a function that will be invoked by the GraphCache when a QoS + /// event is detected. + using GraphCacheEventCallback = std::function; + + /// Set a qos event callback for an entity from the current session. + /// @note The callback will be removed when the entity is removed from the graph. + void set_qos_event_callback( + const liveliness::Entity & entity, + const rmw_zenoh_event_type_t & event_type, + GraphCacheEventCallback callback); + private: - std::string zid_str; + std::string zid_str_; /* namespace_1: node_1: @@ -197,6 +209,18 @@ class GraphCache final // Optimize service/client lookups across the graph. GraphNode::TopicMap graph_services_ = {}; + using GraphEventCallbacks = std::unordered_map; + // Map the liveliness token of an entity to a map of event callbacks. + // Note: Since we use unordered_map, we will only store a single callback for an + // entity string. So we do not support the case where a node create a duplicate + // pub/sub with the exact same topic, type & QoS but registers a different callback + // for the same event type. We could switch to a multimap here but removing the callback + // will be impossible right now since entities do not have unique IDs. + using GraphEventCallbackMap = std::unordered_map; + // EventCallbackMap for each type of event we support in rmw_zenoh_cpp. + GraphEventCallbackMap event_callbacks_; + + // Mutex to lock before modifying the members above. mutable std::mutex graph_mutex_; }; From 33a2160418999e8635e49015760831c81ff2912b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 20 Feb 2024 14:31:15 +0800 Subject: [PATCH 03/12] Store liveliness::Entity in custom types Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 12 +++++++++++ rmw_zenoh_cpp/src/rmw_zenoh.cpp | 24 ++++++++++----------- 2 files changed, 24 insertions(+), 12 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 2a0c18f6..40b05624 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -75,6 +75,9 @@ struct rmw_node_data_t class rmw_publisher_data_t : public EventsBase { public: + // The Entity generated for the publisher. + std::optional entity; + // An owned publisher. z_owned_publisher_t pub; @@ -124,6 +127,9 @@ struct saved_msg_data class rmw_subscription_data_t : public EventsBase { public: + // The Entity generated for the subscription. + std::optional entity; + // An owned subscriber or querying_subscriber depending on the QoS settings. std::variant sub; @@ -183,6 +189,9 @@ class ZenohQuery final class rmw_service_data_t : public EventsBase { public: + // The Entity generated for the service. + std::optional entity; + z_owned_keyexpr_t keyexpr; z_owned_queryable_t qable; @@ -248,6 +257,9 @@ class ZenohReply final class rmw_client_data_t : public EventsBase { public: + // The Entity generated for the client. + std::optional entity; + z_owned_keyexpr_t keyexpr; z_owned_closure_reply_t zn_closure_reply; diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 917a1c02..d4bdf862 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -601,14 +601,14 @@ rmw_create_publisher( z_undeclare_publisher(z_move(publisher_data->pub)); }); - const auto liveliness_entity = liveliness::Entity::make( + publisher_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), liveliness::EntityType::Publisher, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, liveliness::TopicInfo{rmw_publisher->topic_name, publisher_data->type_support->get_name(), publisher_data->adapted_qos_profile} ); - if (!liveliness_entity.has_value()) { + if (!publisher_data->entity.has_value()) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to generate keyexpr for liveliness token for the publisher."); @@ -616,7 +616,7 @@ rmw_create_publisher( } publisher_data->token = zc_liveliness_declare_token( z_loan(node->context->impl->session), - z_keyexpr(liveliness_entity->keyexpr().c_str()), + z_keyexpr(publisher_data->entity->keyexpr().c_str()), NULL ); auto free_token = rcpputils::make_scope_exit( @@ -1344,14 +1344,14 @@ rmw_create_subscription( }); // Publish to the graph that a new subscription is in town - const auto liveliness_entity = liveliness::Entity::make( + sub_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), liveliness::EntityType::Subscription, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, liveliness::TopicInfo{rmw_subscription->topic_name, sub_data->type_support->get_name(), sub_data->adapted_qos_profile} ); - if (!liveliness_entity.has_value()) { + if (!sub_data->entity.has_value()) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to generate keyexpr for liveliness token for the subscription."); @@ -1359,7 +1359,7 @@ rmw_create_subscription( } sub_data->token = zc_liveliness_declare_token( z_loan(context_impl->session), - z_keyexpr(liveliness_entity->keyexpr().c_str()), + z_keyexpr(sub_data->entity->keyexpr().c_str()), NULL ); auto free_token = rcpputils::make_scope_exit( @@ -1986,14 +1986,14 @@ rmw_create_client( service_type.c_str(), rmw_client->service_name); return nullptr; } - const auto liveliness_entity = liveliness::Entity::make( + client_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), liveliness::EntityType::Client, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, liveliness::TopicInfo{rmw_client->service_name, std::move(service_type), client_data->adapted_qos_profile} ); - if (!liveliness_entity.has_value()) { + if (!client_data->entity.has_value()) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to generate keyexpr for liveliness token for the client."); @@ -2001,7 +2001,7 @@ rmw_create_client( } client_data->token = zc_liveliness_declare_token( z_loan(node->context->impl->session), - z_keyexpr(liveliness_entity->keyexpr().c_str()), + z_keyexpr(client_data->entity->keyexpr().c_str()), NULL ); auto free_token = rcpputils::make_scope_exit( @@ -2643,14 +2643,14 @@ rmw_create_service( service_type.c_str(), rmw_service->service_name); return nullptr; } - const auto liveliness_entity = liveliness::Entity::make( + service_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), liveliness::EntityType::Service, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, liveliness::TopicInfo{rmw_service->service_name, std::move(service_type), service_data->adapted_qos_profile} ); - if (!liveliness_entity.has_value()) { + if (!service_data->entity.has_value()) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to generate keyexpr for liveliness token for the service."); @@ -2658,7 +2658,7 @@ rmw_create_service( } service_data->token = zc_liveliness_declare_token( z_loan(node->context->impl->session), - z_keyexpr(liveliness_entity->keyexpr().c_str()), + z_keyexpr(service_data->entity->keyexpr().c_str()), NULL ); auto free_token = rcpputils::make_scope_exit( From f5730530a2bf3ee1596945f29f91a1954c3bc151 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 20 Feb 2024 15:45:26 +0800 Subject: [PATCH 04/12] Register qos event callbacks Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/event.hpp | 1 + rmw_zenoh_cpp/src/detail/graph_cache.hpp | 2 +- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 6 +- rmw_zenoh_cpp/src/rmw_event.cpp | 65 ++++++++++++++++++++- 4 files changed, 69 insertions(+), 5 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/event.hpp b/rmw_zenoh_cpp/src/detail/event.hpp index 727ff1fc..a4505ef3 100644 --- a/rmw_zenoh_cpp/src/detail/event.hpp +++ b/rmw_zenoh_cpp/src/detail/event.hpp @@ -68,6 +68,7 @@ struct rmw_zenoh_event_status_t size_t total_count; size_t total_count_change; size_t current_count; + size_t current_count_change; std::string data; }; diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 7a85bbbd..f1fecff5 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -164,7 +164,7 @@ class GraphCache final /// @brief Signature for a function that will be invoked by the GraphCache when a QoS /// event is detected. - using GraphCacheEventCallback = std::function; + using GraphCacheEventCallback = std::function)>; /// Set a qos event callback for an entity from the current session. /// @note The callback will be removed when the entity is removed from the graph. diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 40b05624..c1d1ea35 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -72,7 +72,8 @@ struct rmw_node_data_t }; ///============================================================================= -class rmw_publisher_data_t : public EventsBase +class rmw_publisher_data_t : public EventsBase, + public std::enable_shared_from_this { public: // The Entity generated for the publisher. @@ -124,7 +125,8 @@ struct saved_msg_data }; ///============================================================================= -class rmw_subscription_data_t : public EventsBase +class rmw_subscription_data_t : public EventsBase, + public std::enable_shared_from_this { public: // The Entity generated for the subscription. diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index 0fde0913..95312839 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -14,9 +14,11 @@ #include "rmw/error_handling.h" #include "rmw/event.h" +#include "rmw/events_statuses/events_statuses.h" #include "rmw/types.h" #include "detail/event.hpp" +#include "detail/graph_cache.hpp" #include "detail/identifier.hpp" #include "detail/rmw_data_types.hpp" @@ -37,13 +39,15 @@ rmw_publisher_event_init( RMW_CHECK_ARGUMENT_FOR_NULL(publisher->data, RMW_RET_INVALID_ARGUMENT); rmw_publisher_data_t * pub_data = static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(pub_data->context, RMW_RET_INVALID_ARGUMENT); if (publisher->implementation_identifier != rmw_zenoh_identifier) { RMW_SET_ERROR_MSG("Publisher implementation identifier not from this implementation"); return RMW_RET_INCORRECT_RMW_IMPLEMENTATION; } - if (event_map.count(event_type) != 1) { + auto rmw_event_it = event_map.find(event_type); + if (rmw_event_it == event_map.end()) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "provided event_type %d is not supported by rmw_zenoh_cpp", event_type); return RMW_RET_UNSUPPORTED; @@ -53,6 +57,24 @@ rmw_publisher_event_init( rmw_event->data = pub_data; rmw_event->event_type = event_type; + // Register the event with graph cache. + pub_data->context->impl->graph_cache->set_qos_event_callback( + pub_data->entity.value(), + rmw_event_it->second, + [pub_data_wk_ptr = pub_data->weak_from_this(), event_id =rmw_event_it->second](std::unique_ptr zenoh_event) + { + auto pub_data = pub_data_wk_ptr.lock(); + if (pub_data == nullptr) { + printf("CANNOT LOCK PUB DATA!!!\n"); + return; + } + printf("Successfully added event %zu from graph cache to event queue\n"); + pub_data->add_new_event( + event_id, + std::move(zenoh_event)); + } + ); + return RMW_RET_OK; } @@ -70,6 +92,7 @@ rmw_subscription_event_init( RMW_CHECK_ARGUMENT_FOR_NULL(subscription->data, RMW_RET_INVALID_ARGUMENT); rmw_subscription_data_t * sub_data = static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(sub_data->context, RMW_RET_INVALID_ARGUMENT); if (subscription->implementation_identifier != rmw_zenoh_identifier) { RMW_SET_ERROR_MSG( @@ -77,7 +100,8 @@ rmw_subscription_event_init( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION; } - if (event_map.count(event_type) != 1) { + auto rmw_event_it = event_map.find(event_type); + if (rmw_event_it == event_map.end()) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "provided event_type %d is not supported by rmw_zenoh_cpp", event_type); return RMW_RET_UNSUPPORTED; @@ -87,6 +111,24 @@ rmw_subscription_event_init( rmw_event->data = sub_data; rmw_event->event_type = event_type; + // Register the event with graph cache. + sub_data->context->impl->graph_cache->set_qos_event_callback( + sub_data->entity.value(), + rmw_event_it->second, + [sub_data_wk_ptr = sub_data->weak_from_this(), event_id =rmw_event_it->second](std::unique_ptr zenoh_event) + { + auto sub_data = sub_data_wk_ptr.lock(); + if (sub_data == nullptr) { + printf("CANNOT LOCK SUB DATA!!!\n"); + return; + } + printf("Successfully added event %zu from graph cache to event queue\n"); + sub_data->add_new_event( + event_id, + std::move(zenoh_event)); + } + ); + return RMW_RET_OK; } @@ -169,6 +211,25 @@ rmw_take_event( *taken = true; return RMW_RET_OK; } + case ZENOH_EVENT_MESSAGE_LOST: { + auto ei = static_cast(event_info); + RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); + ei->total_count = st->total_count; + ei->total_count_change = st->total_count_change; + *taken = true; + return RMW_RET_OK; + } + case ZENOH_EVENT_PUBLICATION_MATCHED: + case ZENOH_EVENT_SUBSCRIPTION_MATCHED: { + auto ei = static_cast(event_info); + RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); + ei->total_count = st->total_count; + ei->total_count_change = st->total_count_change; + ei->current_count = st->current_count; + ei->current_count_change = st->current_count_change; + *taken = true; + return RMW_RET_OK; + } case ZENOH_EVENT_OFFERED_QOS_INCOMPATIBLE: { auto ei = static_cast(event_info); RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); From b4e645a78eb6df6041dd955b4678192f3c019a97 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 21 Feb 2024 00:11:34 +0800 Subject: [PATCH 05/12] Remove all lambdas in graph cache Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 529 ++++++++++---------- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 32 ++ rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 2 + rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 6 +- rmw_zenoh_cpp/src/rmw_event.cpp | 15 +- 5 files changed, 315 insertions(+), 269 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 94dcc847..d7741b3f 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -58,142 +58,145 @@ TopicData::TopicData( ///============================================================================= GraphCache::GraphCache(const z_id_t & zid) -: zid_str_(std::move(liveliness::zid_to_str(zid))) +: zid_str_(liveliness::zid_to_str(zid)) { // Do nothing. } ///============================================================================= -void GraphCache::parse_put(const std::string & keyexpr) +std::shared_ptr GraphCache::make_graph_node(const Entity & entity) const { - std::optional valid_entity = liveliness::Entity::make(keyexpr); - if (!valid_entity.has_value()) { - // Error message has already been logged. + if (entity.type() == EntityType::Invalid) { + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "make_graph_node() called for Invalid entity. Report this."); + return nullptr; + } + auto graph_node = std::make_shared(); + graph_node->id_ = entity.id(); + graph_node->ns_ = entity.node_namespace(); + graph_node->name_ = entity.node_name(); + graph_node->enclave_ = entity.node_enclave(); + + return graph_node; +} + +///============================================================================= +void GraphCache::update_topic_maps_for_put( + GraphNodePtr graph_node, + const liveliness::Entity & entity) +{ + if (entity.type() == EntityType::Node) { + // Nothing to update for a node entity. return; } - const liveliness::Entity & entity = *valid_entity; - // Helper lambda to append pub/subs to the GraphNode. - // We capture by reference to update graph_topics_ - auto add_topic_data = - [](const Entity & entity, GraphNode & graph_node, GraphCache & graph_cache) -> void - { - if (entity.type() == EntityType::Invalid || - entity.type() == EntityType::Node) - { - RCUTILS_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "add_topic_data() for invalid EntityType. Report this."); - return; - } + // Add endpoint entries. + // Append pub/subs to the GraphNode. + if (!entity.topic_info().has_value()) { + // This should not happen as topic_info should be populated for all non-node entites. + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "update_topic_maps_for_put() called for non-node entity without valid TopicInfo. " + "Report this."); + return; + } - if (!entity.topic_info().has_value()) { - // This should not happen as add_topic_data() is called after validating the existence - // of topic_info. - RCUTILS_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "add_topic_data() called without valid TopicInfo. Report this."); - return; - } + const liveliness::TopicInfo topic_info = entity.topic_info().value(); + // For the sake of reusing data structures and lookup functions, we treat publishers and + // clients are equivalent. Similarly, subscriptions and services are equivalent. + const std::size_t pub_count = entity.type() == EntityType::Publisher || + entity.type() == EntityType::Client ? 1 : 0; + const std::size_t sub_count = !pub_count; + + // First update the topic map within the node. + if (entity.type() == EntityType::Publisher) { + update_topic_map_for_put(graph_node->pubs_, topic_info, pub_count, sub_count); + } else if (entity.type() == EntityType::Subscription) { + update_topic_map_for_put(graph_node->subs_, topic_info, pub_count, sub_count); + } else if (entity.type() == EntityType::Service) { + update_topic_map_for_put(graph_node->services_, topic_info, pub_count, sub_count); + } else { + update_topic_map_for_put(graph_node->clients_, topic_info, pub_count, sub_count); + } - const liveliness::TopicInfo topic_info = entity.topic_info().value(); - // For the sake of reusing data structures and lookup functions, we treat publishers and - // clients are equivalent. Similarly, subscriptions and services are equivalent. - const std::size_t pub_count = entity.type() == EntityType::Publisher || - entity.type() == EntityType::Client ? 1 : 0; - const std::size_t sub_count = !pub_count; - - // Helper lambda to update TopicMap within the node the one for the entire graph. - auto update_topic_map = - [](GraphNode::TopicMap & topic_map, - const liveliness::TopicInfo topic_info, - const std::size_t pub_count, - const std::size_t sub_count) -> void - { - TopicDataPtr graph_topic_data = std::make_shared( - topic_info, - TopicStats{pub_count, sub_count}); - std::string qos_str = liveliness::qos_to_keyexpr(topic_info.qos_); - GraphNode::TopicQoSMap topic_qos_map = { - {qos_str, graph_topic_data}}; - - GraphNode::TopicMap::iterator topic_map_it = topic_map.find(topic_info.name_); - if (topic_map_it == topic_map.end()) { - // First time this topic name is discovered for the node so we insert a TopicTypeMap. - GraphNode::TopicTypeMap topic_data_map = { - {topic_info.type_, std::move(topic_qos_map)} - }; - topic_map.insert(std::make_pair(topic_info.name_, std::move(topic_data_map))); - } else { - // The topic exists for the node so we check if the type also exists. - GraphNode::TopicTypeMap::iterator topic_data_map_it = topic_map_it->second.find( - topic_info.type_); - if (topic_data_map_it == topic_map_it->second.end()) { - // First time this topic type is added. - topic_map_it->second.insert( - std::make_pair( - topic_info.type_, - std::move(topic_qos_map))); - } else { - // The topic type already exists so we check if qos also exists. - GraphNode::TopicQoSMap::iterator topic_qos_map_it = topic_data_map_it->second.find( - qos_str); - if (topic_qos_map_it == topic_data_map_it->second.end()) { - // First time this qos is added. - topic_data_map_it->second.insert(std::make_pair(qos_str, graph_topic_data)); - } else { - // We have another instance of a pub/sub over the same topic, - // type and qos so we increment the counters. - TopicDataPtr & existing_graph_topic = topic_qos_map_it->second; - existing_graph_topic->stats_.pub_count_ += pub_count; - existing_graph_topic->stats_.sub_count_ += sub_count; - } - } - } - }; - - // First update the topic map within the node. - if (entity.type() == EntityType::Publisher) { - update_topic_map(graph_node.pubs_, topic_info, pub_count, sub_count); - } else if (entity.type() == EntityType::Subscription) { - update_topic_map(graph_node.subs_, topic_info, pub_count, sub_count); - } else if (entity.type() == EntityType::Service) { - update_topic_map(graph_node.services_, topic_info, pub_count, sub_count); - } else { - update_topic_map(graph_node.clients_, topic_info, pub_count, sub_count); - } + // Then update the variables tracking topics across the graph. + // TODO(Yadunund): Check for QoS events. + if (entity.type() == EntityType::Publisher || + entity.type() == EntityType::Subscription) + { + update_topic_map_for_put(this->graph_topics_, topic_info, pub_count, sub_count); + } else { + update_topic_map_for_put(this->graph_services_, topic_info, pub_count, sub_count); + } +} - // Then update the variables tracking topics across the graph. - // TODO(Yadunund): Check for QoS events. - if (entity.type() == EntityType::Publisher || - entity.type() == EntityType::Subscription) - { - update_topic_map(graph_cache.graph_topics_, topic_info, pub_count, sub_count); +///============================================================================= +void GraphCache::update_topic_map_for_put( + GraphNode::TopicMap & topic_map, + const liveliness::TopicInfo & topic_info, + const std::size_t pub_count, + const std::size_t sub_count) +{ + TopicDataPtr graph_topic_data = std::make_shared( + topic_info, + TopicStats{pub_count, sub_count}); + std::string qos_str = liveliness::qos_to_keyexpr(topic_info.qos_); + GraphNode::TopicQoSMap topic_qos_map = { + {qos_str, graph_topic_data}}; + + GraphNode::TopicMap::iterator topic_map_it = topic_map.find(topic_info.name_); + if (topic_map_it == topic_map.end()) { + // First time this topic name is discovered for the node so we insert a TopicTypeMap. + GraphNode::TopicTypeMap topic_data_map = { + {topic_info.type_, std::move(topic_qos_map)} + }; + topic_map.insert(std::make_pair(topic_info.name_, std::move(topic_data_map))); + } else { + // The topic exists for the node so we check if the type also exists. + GraphNode::TopicTypeMap::iterator topic_data_map_it = topic_map_it->second.find( + topic_info.type_); + if (topic_data_map_it == topic_map_it->second.end()) { + // First time this topic type is added. + topic_map_it->second.insert( + std::make_pair( + topic_info.type_, + std::move(topic_qos_map))); + } else { + // The topic type already exists so we check if qos also exists. + GraphNode::TopicQoSMap::iterator topic_qos_map_it = topic_data_map_it->second.find( + qos_str); + if (topic_qos_map_it == topic_data_map_it->second.end()) { + // First time this qos is added. + topic_data_map_it->second.insert(std::make_pair(qos_str, graph_topic_data)); } else { - update_topic_map(graph_cache.graph_services_, topic_info, pub_count, sub_count); + // We have another instance of a pub/sub over the same topic, + // type and qos so we increment the counters. + TopicDataPtr & existing_graph_topic = topic_qos_map_it->second; + existing_graph_topic->stats_.pub_count_ += pub_count; + existing_graph_topic->stats_.sub_count_ += sub_count; } - }; + } + } +} - // Helper lambda to convert an Entity into a GraphNode. - // Note: this will update bookkeeping variables in GraphCache. - auto make_graph_node = - [&add_topic_data](const Entity & entity, GraphCache & graph_cache) -> std::shared_ptr - { - auto graph_node = std::make_shared(); - graph_node->id_ = entity.id(); - graph_node->ns_ = entity.node_namespace(); - graph_node->name_ = entity.node_name(); - graph_node->enclave_ = entity.node_enclave(); - - if (!entity.topic_info().has_value()) { - // Token was for a node. - return graph_node; - } - // Add endpoint entries. - add_topic_data(entity, *graph_node, graph_cache); +///============================================================================= +void GraphCache::parse_put(const std::string & keyexpr) +{ + std::optional maybe_entity = liveliness::Entity::make(keyexpr); + if (!maybe_entity.has_value()) { + // Error message has already been logged. + return; + } - return graph_node; - }; + const liveliness::Entity & entity = *maybe_entity; + + if (entity.type() == EntityType::Invalid) { + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "parse_put() called for an invalid entity. Report this."); + return; + } // Lock the graph mutex before accessing the graph. std::lock_guard lock(graph_mutex_); @@ -201,9 +204,15 @@ void GraphCache::parse_put(const std::string & keyexpr) // If the namespace did not exist, create it and add the node to the graph and return. NamespaceMap::iterator ns_it = graph_.find(entity.node_namespace()); if (ns_it == graph_.end()) { + GraphNodePtr node = make_graph_node(entity); + if (node == nullptr) { + // Error handled. + return; + } NodeMap node_map = { - {entity.node_name(), make_graph_node(entity, *this)}}; + {entity.node_name(), node}}; graph_.emplace(std::make_pair(entity.node_namespace(), std::move(node_map))); + update_topic_maps_for_put(node, entity); total_nodes_in_graph_ += 1; return; } @@ -224,8 +233,14 @@ void GraphCache::parse_put(const std::string & keyexpr) if (node_it == range.second) { // Either the first time a node with this name is added or with an existing // name but unique id. + GraphNodePtr node = make_graph_node(entity); + if (node == nullptr) { + // Error handled. + return; + } NodeMap::iterator insertion_it = - ns_it->second.insert(std::make_pair(entity.node_name(), make_graph_node(entity, *this))); + ns_it->second.insert(std::make_pair(entity.node_name(), node)); + update_topic_maps_for_put(node, entity); total_nodes_in_graph_ += 1; if (insertion_it == ns_it->second.end()) { RCUTILS_LOG_ERROR_NAMED( @@ -240,129 +255,142 @@ void GraphCache::parse_put(const std::string & keyexpr) } // Otherwise, the entity represents a node that already exists in the graph. // Update topic info if required below. + update_topic_maps_for_put(node_it->second, entity); +} - // Handles additions to an existing node in the graph. +///============================================================================= +void GraphCache::update_topic_maps_for_del( + GraphNodePtr graph_node, + const liveliness::Entity & entity) +{ if (entity.type() == EntityType::Node) { - // Creating a new node above would have also updated the graph with any topic info. + // Nothing to update for a node entity. return; } if (!entity.topic_info().has_value()) { - // Likely an error with parsing the token. - RCUTILS_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "Put token %s parsed without extracting topic_info. Report this bug.", - keyexpr.c_str()); + // This should not happen as add_topic_data() is called after validating the existence + // of topic_info. + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "update_topic_maps_for_del() called for non-node entity without valid TopicInfo. " + "Report this."); return; } - // Update the graph based on the entity. - add_topic_data(entity, *(node_it->second), *this); + const liveliness::TopicInfo topic_info = entity.topic_info().value(); + // For the sake of reusing data structures and lookup functions, we treat publishers and + // clients are equivalent. Similarly, subscriptions and services are equivalent. + const std::size_t pub_count = entity.type() == EntityType::Publisher || + entity.type() == EntityType::Client ? 1 : 0; + const std::size_t sub_count = !pub_count; + + // First update the topic map within the node. + if (entity.type() == EntityType::Publisher) { + update_topic_map_for_del(graph_node->pubs_, topic_info, pub_count, sub_count); + } else if (entity.type() == EntityType::Subscription) { + update_topic_map_for_del(graph_node->subs_, topic_info, pub_count, sub_count); + } else if (entity.type() == EntityType::Service) { + update_topic_map_for_del(graph_node->services_, topic_info, pub_count, sub_count); + } else { + update_topic_map_for_del(graph_node->clients_, topic_info, pub_count, sub_count); + } + + // Then update the variables tracking topics across the graph. + // TODO(Yadunund): Check for QoS events. + if (entity.type() == EntityType::Publisher || + entity.type() == EntityType::Subscription) + { + update_topic_map_for_del(this->graph_topics_, topic_info, pub_count, sub_count); + } else { + update_topic_map_for_del(this->graph_services_, topic_info, pub_count, sub_count); + } } ///============================================================================= -void GraphCache::parse_del(const std::string & keyexpr) +void GraphCache::update_topic_map_for_del( + GraphNode::TopicMap & topic_map, + const liveliness::TopicInfo & topic_info, + const std::size_t pub_count, + const std::size_t sub_count) { - std::optional valid_entity = liveliness::Entity::make(keyexpr); - if (!valid_entity.has_value()) { - // Error message has already been logged. - return; - } - const liveliness::Entity & entity = *valid_entity; - - // Helper lambda to update graph_topics_. - auto update_topic_map = - [](GraphNode::TopicMap & graph_endpoints, - const liveliness::TopicInfo & topic_info, - std::size_t pub_count, - std::size_t sub_count) -> void - { - GraphNode::TopicMap::iterator cache_topic_it = - graph_endpoints.find(topic_info.name_); - if (cache_topic_it == graph_endpoints.end()) { - // This should not happen. - RCUTILS_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "topic_key %s not found in graph_endpoints. Report this.", - topic_info.name_.c_str()); - } else { - GraphNode::TopicTypeMap::iterator cache_topic_data_it = - cache_topic_it->second.find(topic_info.type_); - if (cache_topic_data_it != cache_topic_it->second.end()) { - const std::string qos_str = liveliness::qos_to_keyexpr(topic_info.qos_); - GraphNode::TopicQoSMap::iterator cache_topic_qos_it = cache_topic_data_it->second.find( - qos_str); - if (cache_topic_qos_it != cache_topic_data_it->second.end()) { - // Decrement the relevant counters. If both counters are 0 remove from cache. - cache_topic_qos_it->second->stats_.pub_count_ -= pub_count; - cache_topic_qos_it->second->stats_.sub_count_ -= sub_count; - if (cache_topic_qos_it->second->stats_.pub_count_ == 0 && - cache_topic_qos_it->second->stats_.sub_count_ == 0) - { - cache_topic_data_it->second.erase(qos_str); - } - // If the qos map is empty, erase it from the topic_data_map. - if (cache_topic_data_it->second.empty()) { - cache_topic_it->second.erase(cache_topic_data_it); - } - } - // If the topic does not have any TopicData entries, erase the topic from the map. - if (cache_topic_it->second.empty()) { - graph_endpoints.erase(cache_topic_it); - } + GraphNode::TopicMap::iterator cache_topic_it = + topic_map.find(topic_info.name_); + if (cache_topic_it == topic_map.end()) { + // This should not happen. + RCUTILS_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "topic_key %s not found in topic_map. Report this.", + topic_info.name_.c_str()); + } else { + GraphNode::TopicTypeMap::iterator cache_topic_data_it = + cache_topic_it->second.find(topic_info.type_); + if (cache_topic_data_it != cache_topic_it->second.end()) { + const std::string qos_str = liveliness::qos_to_keyexpr(topic_info.qos_); + GraphNode::TopicQoSMap::iterator cache_topic_qos_it = cache_topic_data_it->second.find( + qos_str); + if (cache_topic_qos_it != cache_topic_data_it->second.end()) { + // Decrement the relevant counters. If both counters are 0 remove from cache. + cache_topic_qos_it->second->stats_.pub_count_ -= pub_count; + cache_topic_qos_it->second->stats_.sub_count_ -= sub_count; + if (cache_topic_qos_it->second->stats_.pub_count_ <= 0 && + cache_topic_qos_it->second->stats_.sub_count_ <= 0) + { + cache_topic_data_it->second.erase(qos_str); + } + // If the qos map is empty, erase it from the topic_data_map. + if (cache_topic_data_it->second.empty()) { + cache_topic_it->second.erase(cache_topic_data_it); } } - }; + // If the topic does not have any TopicData entries, erase the topic from the map. + if (cache_topic_it->second.empty()) { + topic_map.erase(cache_topic_it); + } + } + } +} - // Helper lambda to remove pub/subs to the GraphNode. - // We capture by reference to update caches like graph_topics_ if update_cache is true. - auto remove_topic_data = - [&update_topic_map](const Entity & entity, GraphNode & graph_node, - GraphCache & graph_cache) -> void +///============================================================================= +void GraphCache::remove_topic_map_from_cache( + const GraphNode::TopicMap & to_remove, + GraphNode::TopicMap & from_cache) +{ + for (GraphNode::TopicMap::const_iterator topic_it = to_remove.begin(); + topic_it != to_remove.end(); ++topic_it) + { + for (GraphNode::TopicTypeMap::const_iterator topic_type_it = topic_it->second.begin(); + topic_type_it != topic_it->second.end(); ++topic_type_it) { - if (entity.type() == EntityType::Invalid || - entity.type() == EntityType::Node) + for (GraphNode::TopicQoSMap::const_iterator topic_qos_it = + topic_type_it->second.begin(); + topic_qos_it != topic_type_it->second.end(); ++topic_qos_it) { - RCUTILS_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "remove_topic_data() for invalid EntityType. Report this."); - return; + update_topic_map_for_del( + from_cache, + topic_qos_it->second->info_, + topic_qos_it->second->stats_.pub_count_, + topic_qos_it->second->stats_.sub_count_); } + } + } +} - if (!entity.topic_info().has_value()) { - // This should not happen as add_topic_data() is called after validating the existence - // of topic_info. - RCUTILS_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "remove_topic_data() called without valid TopicInfo. Report this."); - return; - } - const liveliness::TopicInfo topic_info = entity.topic_info().value(); - // For the sake of reusing data structures and lookup functions, we treat publishers and - // clients are equivalent. Similarly, subscriptions and services are equivalent. - const std::size_t pub_count = entity.type() == EntityType::Publisher || - entity.type() == EntityType::Client ? 1 : 0; - const std::size_t sub_count = !pub_count; - - // First update the topic map within the node. - if (entity.type() == EntityType::Publisher) { - update_topic_map(graph_node.pubs_, topic_info, pub_count, sub_count); - } else if (entity.type() == EntityType::Subscription) { - update_topic_map(graph_node.subs_, topic_info, pub_count, sub_count); - } else if (entity.type() == EntityType::Service) { - update_topic_map(graph_node.services_, topic_info, pub_count, sub_count); - } else { - update_topic_map(graph_node.clients_, topic_info, pub_count, sub_count); - } +///============================================================================= +void GraphCache::parse_del(const std::string & keyexpr) +{ + std::optional maybe_entity = liveliness::Entity::make(keyexpr); + if (!maybe_entity.has_value()) { + // Error message has already been logged. + return; + } + const liveliness::Entity entity = *maybe_entity; - // Then update the variables tracking topics across the graph. - // TODO(Yadunund): Check for QoS events. - if (entity.type() == EntityType::Publisher || - entity.type() == EntityType::Subscription) - { - update_topic_map(graph_cache.graph_topics_, topic_info, pub_count, sub_count); - } else { - update_topic_map(graph_cache.graph_services_, topic_info, pub_count, sub_count); - } - }; + if (entity.type() == EntityType::Invalid) { + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "parse_del() called for an invalid entity. Report this."); + return; + } // Lock the graph mutex before accessing the graph. std::lock_guard lock(graph_mutex_); @@ -380,7 +408,6 @@ void GraphCache::parse_del(const std::string & keyexpr) range.first, range.second, [&entity](const std::pair & node_it) { - // An operator== overload is defined above. return entity.id() == node_it.second->id_; }); if (node_it == range.second) { @@ -399,37 +426,18 @@ void GraphCache::parse_del(const std::string & keyexpr) // given the reliability QoS for liveliness subs. However, if we find any pubs/subs present in // the node below, we should update the count in graph_topics_. const GraphNodePtr graph_node = node_it->second; - if (!graph_node->pubs_.empty() || !graph_node->subs_.empty()) { + if (!graph_node->pubs_.empty() || + !graph_node->subs_.empty() || + !graph_node->clients_.empty() || + !graph_node->services_.empty()) + { RCUTILS_LOG_WARN_NAMED( "rmw_zenoh_cpp", - "Received liveliness token to remove node /%s from the graph before all pub/subs for this " - "node have been removed. Removing all pub/subs first...", + "Received liveliness token to remove node /%s from the graph before all pub/subs/" + "clients/services for this node have been removed. Removing all entities first...", entity.node_name().c_str() ); // We update the tracking variables to reduce the count of entities present in this node. - auto remove_topic_map_from_cache = - [&update_topic_map](const GraphNode::TopicMap & to_remove, - GraphNode::TopicMap & from_cache) -> void - { - for (GraphNode::TopicMap::const_iterator topic_it = to_remove.begin(); - topic_it != to_remove.end(); ++topic_it) - { - for (GraphNode::TopicTypeMap::const_iterator topic_type_it = topic_it->second.begin(); - topic_type_it != topic_it->second.end(); ++topic_type_it) - { - for (GraphNode::TopicQoSMap::const_iterator topic_qos_it = - topic_type_it->second.begin(); - topic_qos_it != topic_type_it->second.end(); ++topic_qos_it) - { - update_topic_map( - from_cache, - topic_qos_it->second->info_, - topic_qos_it->second->stats_.pub_count_, - topic_qos_it->second->stats_.sub_count_); - } - } - } - }; remove_topic_map_from_cache(graph_node->pubs_, graph_topics_); remove_topic_map_from_cache(graph_node->subs_, graph_topics_); remove_topic_map_from_cache(graph_node->services_, graph_services_); @@ -443,16 +451,8 @@ void GraphCache::parse_del(const std::string & keyexpr) return; } - if (!entity.topic_info().has_value()) { - // Likely an error with parsing the token. - RCUTILS_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "Del token %s parsed without extracting TopicData. Report this bug.", - keyexpr.c_str()); - return; - } - // Update the graph based on the entity. - remove_topic_data(entity, *(node_it->second), *this); + update_topic_maps_for_del(node_it->second, entity); } ///============================================================================= @@ -1059,3 +1059,12 @@ void GraphCache::set_qos_event_callback( } event_cb_it->second.insert(std::make_pair(event_type, std::move(callback))); } + +///============================================================================= +bool GraphCache::is_local_entity(const liveliness::Entity & entity) const +{ + // For now zenoh does not expose unique IDs for its entities and hence the id + // assigned to an entity is always the zenoh session id. When we update liveliness + // tokens to contain globally unique ids for entities, we should also update the logic here. + return entity.id() == zid_str_; +} diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index f1fecff5..16b97003 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -173,7 +173,39 @@ class GraphCache final const rmw_zenoh_event_type_t & event_type, GraphCacheEventCallback callback); + /// Returns true if the entity was created within the same context / zenoh session. + bool is_local_entity(const liveliness::Entity & entity) const; + private: + // Helper function to convert an Entity into a GraphNode. + // Note: this will update bookkeeping variables in GraphCache. + std::shared_ptr make_graph_node(const liveliness::Entity & entity) const; + + // Helper function to update TopicMap within the node the cache for the entire graph. + void update_topic_maps_for_put( + GraphNodePtr graph_node, + const liveliness::Entity & entity); + + void update_topic_map_for_put( + GraphNode::TopicMap & topic_map, + const liveliness::TopicInfo & topic_info, + const std::size_t pub_count, + const std::size_t sub_count); + + void update_topic_maps_for_del( + GraphNodePtr graph_node, + const liveliness::Entity & entity); + + void update_topic_map_for_del( + GraphNode::TopicMap & topic_map, + const liveliness::TopicInfo & topic_info, + const std::size_t pub_count, + const std::size_t sub_count); + + void remove_topic_map_from_cache( + const GraphNode::TopicMap & to_remove, + GraphNode::TopicMap & from_cache); + std::string zid_str_; /* namespace_1: diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index cebb27bb..28023fb5 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -111,6 +111,8 @@ void rmw_subscription_data_t::add_new_message( } } + // TODO(Yadunund): Check for ZENOH_EVENT_MESSAGE_LOST. + message_queue_.emplace_back(std::move(msg)); // Since we added new data, trigger user callback and guard condition if they are available diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index c1d1ea35..40b05624 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -72,8 +72,7 @@ struct rmw_node_data_t }; ///============================================================================= -class rmw_publisher_data_t : public EventsBase, - public std::enable_shared_from_this +class rmw_publisher_data_t : public EventsBase { public: // The Entity generated for the publisher. @@ -125,8 +124,7 @@ struct saved_msg_data }; ///============================================================================= -class rmw_subscription_data_t : public EventsBase, - public std::enable_shared_from_this +class rmw_subscription_data_t : public EventsBase { public: // The Entity generated for the subscription. diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index 95312839..b0c204c3 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -61,9 +61,9 @@ rmw_publisher_event_init( pub_data->context->impl->graph_cache->set_qos_event_callback( pub_data->entity.value(), rmw_event_it->second, - [pub_data_wk_ptr = pub_data->weak_from_this(), event_id =rmw_event_it->second](std::unique_ptr zenoh_event) + [pub_data, + event_id = rmw_event_it->second](std::unique_ptr zenoh_event) { - auto pub_data = pub_data_wk_ptr.lock(); if (pub_data == nullptr) { printf("CANNOT LOCK PUB DATA!!!\n"); return; @@ -111,13 +111,18 @@ rmw_subscription_event_init( rmw_event->data = sub_data; rmw_event->event_type = event_type; - // Register the event with graph cache. + // Register the event with graph cache if the event is not ZENOH_EVENT_MESSAGE_LOST + // since this is checked for in the subscription callback. + if (rmw_event_it->second == ZENOH_EVENT_MESSAGE_LOST) { + return RMW_RET_OK; + } + sub_data->context->impl->graph_cache->set_qos_event_callback( sub_data->entity.value(), rmw_event_it->second, - [sub_data_wk_ptr = sub_data->weak_from_this(), event_id =rmw_event_it->second](std::unique_ptr zenoh_event) + [sub_data, + event_id = rmw_event_it->second](std::unique_ptr zenoh_event) { - auto sub_data = sub_data_wk_ptr.lock(); if (sub_data == nullptr) { printf("CANNOT LOCK SUB DATA!!!\n"); return; From 395bd3177ae3857af5c4d9bc5c7ccd59262da824 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 23 Feb 2024 11:43:35 +0800 Subject: [PATCH 06/12] Implement matched events Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/event.hpp | 10 +- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 456 +++++++++++++----- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 56 ++- rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 15 + rmw_zenoh_cpp/src/detail/liveliness_utils.hpp | 40 +- rmw_zenoh_cpp/src/rmw_event.cpp | 10 +- 6 files changed, 450 insertions(+), 137 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/event.hpp b/rmw_zenoh_cpp/src/detail/event.hpp index a4505ef3..be065041 100644 --- a/rmw_zenoh_cpp/src/detail/event.hpp +++ b/rmw_zenoh_cpp/src/detail/event.hpp @@ -36,14 +36,14 @@ enum rmw_zenoh_event_type_t // subscription events ZENOH_EVENT_REQUESTED_QOS_INCOMPATIBLE, ZENOH_EVENT_MESSAGE_LOST, - // RMW_EVENT_SUBSCRIPTION_INCOMPATIBLE_TYPE, + ZENOH_EVENT_SUBSCRIPTION_INCOMPATIBLE_TYPE, ZENOH_EVENT_SUBSCRIPTION_MATCHED, // publisher events // RMW_EVENT_LIVELINESS_LOST, // RMW_EVENT_OFFERED_DEADLINE_MISSED, ZENOH_EVENT_OFFERED_QOS_INCOMPATIBLE, - // RMW_EVENT_PUBLISHER_INCOMPATIBLE_TYPE, + ZENOH_EVENT_PUBLISHER_INCOMPATIBLE_TYPE, ZENOH_EVENT_PUBLICATION_MATCHED, }; @@ -56,19 +56,21 @@ static const std::unordered_map event_ {RMW_EVENT_OFFERED_QOS_INCOMPATIBLE, ZENOH_EVENT_OFFERED_QOS_INCOMPATIBLE}, {RMW_EVENT_MESSAGE_LOST, ZENOH_EVENT_MESSAGE_LOST}, {RMW_EVENT_SUBSCRIPTION_MATCHED, ZENOH_EVENT_SUBSCRIPTION_MATCHED}, - {RMW_EVENT_PUBLICATION_MATCHED, ZENOH_EVENT_PUBLICATION_MATCHED} + {RMW_EVENT_PUBLICATION_MATCHED, ZENOH_EVENT_PUBLICATION_MATCHED}, + {RMW_EVENT_SUBSCRIPTION_INCOMPATIBLE_TYPE, ZENOH_EVENT_SUBSCRIPTION_INCOMPATIBLE_TYPE}, + {RMW_EVENT_PUBLISHER_INCOMPATIBLE_TYPE, ZENOH_EVENT_PUBLISHER_INCOMPATIBLE_TYPE} // TODO(clalancette): Implement remaining events }; ///============================================================================= /// A struct to store status changes which can be mapped to rmw event statuses. -/// The data field can be used to store serialized information for more complex statuses. struct rmw_zenoh_event_status_t { size_t total_count; size_t total_count_change; size_t current_count; size_t current_count_change; + // The data field can be used to store serialized information for more complex statuses. std::string data; }; diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index d7741b3f..5e90e38a 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -40,14 +40,6 @@ using Entity = liveliness::Entity; using EntityType = liveliness::EntityType; -///============================================================================= -TopicStats::TopicStats(std::size_t pub_count, std::size_t sub_count) -: pub_count_(pub_count), - sub_count_(sub_count) -{ - // Do nothing. -} - ///============================================================================= TopicData::TopicData( liveliness::TopicInfo info, @@ -91,56 +83,62 @@ void GraphCache::update_topic_maps_for_put( return; } - // Add endpoint entries. - // Append pub/subs to the GraphNode. - if (!entity.topic_info().has_value()) { - // This should not happen as topic_info should be populated for all non-node entites. - RCUTILS_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "update_topic_maps_for_put() called for non-node entity without valid TopicInfo. " - "Report this."); - return; - } - - const liveliness::TopicInfo topic_info = entity.topic_info().value(); - // For the sake of reusing data structures and lookup functions, we treat publishers and - // clients are equivalent. Similarly, subscriptions and services are equivalent. - const std::size_t pub_count = entity.type() == EntityType::Publisher || - entity.type() == EntityType::Client ? 1 : 0; - const std::size_t sub_count = !pub_count; - // First update the topic map within the node. if (entity.type() == EntityType::Publisher) { - update_topic_map_for_put(graph_node->pubs_, topic_info, pub_count, sub_count); + update_topic_map_for_put(graph_node->pubs_, entity); } else if (entity.type() == EntityType::Subscription) { - update_topic_map_for_put(graph_node->subs_, topic_info, pub_count, sub_count); + update_topic_map_for_put(graph_node->subs_, entity); } else if (entity.type() == EntityType::Service) { - update_topic_map_for_put(graph_node->services_, topic_info, pub_count, sub_count); + update_topic_map_for_put(graph_node->services_, entity); } else { - update_topic_map_for_put(graph_node->clients_, topic_info, pub_count, sub_count); + update_topic_map_for_put(graph_node->clients_, entity); } // Then update the variables tracking topics across the graph. - // TODO(Yadunund): Check for QoS events. + // We invoke update_topic_map_for_put() with report_events set to true for + // pub/sub. if (entity.type() == EntityType::Publisher || entity.type() == EntityType::Subscription) { - update_topic_map_for_put(this->graph_topics_, topic_info, pub_count, sub_count); + update_topic_map_for_put(this->graph_topics_, entity, true); } else { - update_topic_map_for_put(this->graph_services_, topic_info, pub_count, sub_count); + update_topic_map_for_put(this->graph_services_, entity); } } ///============================================================================= void GraphCache::update_topic_map_for_put( GraphNode::TopicMap & topic_map, - const liveliness::TopicInfo & topic_info, - const std::size_t pub_count, - const std::size_t sub_count) + const liveliness::Entity & entity, + bool report_events) { + if (!entity.topic_info().has_value()) { + // This should not happen as topic_info should be populated for all non-node entites. + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "update_topic_map_for_put() called for non-node entity without valid TopicInfo. " + "Report this."); + return; + } + const liveliness::TopicInfo topic_info = entity.topic_info().value(); + + // For the sake of reusing data structures and lookup functions, we treat publishers and + // clients as equivalent. Similarly, subscriptions and services are equivalent. + const bool is_pub = is_entity_pub(entity); + // Initialize a map that will be populated with any QoS events that may be detected. + std::unordered_map> + local_entities_with_events = {}; + + TopicStats new_topic_stats; + if (is_pub) { + new_topic_stats.pubs_.insert(entity); + } else { + new_topic_stats.subs_.insert(entity); + } TopicDataPtr graph_topic_data = std::make_shared( topic_info, - TopicStats{pub_count, sub_count}); + std::move(new_topic_stats)); + std::string qos_str = liveliness::qos_to_keyexpr(topic_info.qos_); GraphNode::TopicQoSMap topic_qos_map = { {qos_str, graph_topic_data}}; @@ -154,27 +152,147 @@ void GraphCache::update_topic_map_for_put( topic_map.insert(std::make_pair(topic_info.name_, std::move(topic_data_map))); } else { // The topic exists for the node so we check if the type also exists. - GraphNode::TopicTypeMap::iterator topic_data_map_it = topic_map_it->second.find( + GraphNode::TopicTypeMap::iterator topic_type_map_it = topic_map_it->second.find( topic_info.type_); - if (topic_data_map_it == topic_map_it->second.end()) { + if (topic_type_map_it == topic_map_it->second.end()) { // First time this topic type is added. + + // Check for and report an *_INCOMPATIBLE_TYPE event if a different type for the same + // topic exists. +// if (report_events) { +// for (const auto & [topic_type, qos_map] : topic_map_it->second) { +// for (const auto & [qos_type, topic_data_ptr] : qos_map) { +// // If the entity is a publisher but the graph contains a sub, +// // report ZENOH_EVENT_PUBLISHER_INCOMPATIBLE_TYPE or +// // ZENOH_EVENT_PUBLISHER_INCOMPATIBLE_TYPE if vice versa. +// if ((is_pub && topic_data_ptr->stats_.sub_count_ > 0) || +// (!is_pub && topic_data_ptr->stats_.pub_count_ > 0)) +// { +// goto incompatible_type_event_found; +// } +// } +// } +// incompatible_type_event_found: +// // TODO(Yadunund): Retrieve total count from global counters. +// auto event_type = is_pub ? ZENOH_EVENT_PUBLISHER_INCOMPATIBLE_TYPE : +// ZENOH_EVENT_SUBSCRIPTION_INCOMPATIBLE_TYPE; +// auto event = std::make_unique(); +// event->total_count = 1; +// event->total_count_change = 1; +// detected_events[event_type] = std::move(event); +// } + topic_map_it->second.insert( std::make_pair( topic_info.type_, std::move(topic_qos_map))); } else { - // The topic type already exists so we check if qos also exists. - GraphNode::TopicQoSMap::iterator topic_qos_map_it = topic_data_map_it->second.find( + // The topic type already exists. + // With Zenoh, as long as topic name and type match, transport will ensure + // payloads are received by subs. Hence, we can check for matched events + // without having to check for any qos compatibilities. + if (report_events) { + // The entity added may be local with callbacks registered but there + // may be other local entities in the graph that are matched. + std::size_t match_count_for_entity = 0; + for (const auto & [_, topic_data_ptr] : topic_type_map_it->second) { + if (is_pub) { + // Count the number of matching subs for each set of qos settings. + if (!topic_data_ptr->stats_.subs_.empty()) { + match_count_for_entity += topic_data_ptr->stats_.subs_.size(); + } + // Also iterate through the subs to check if any are local and if update event counters. + for (const liveliness::Entity & sub_entity : topic_data_ptr->stats_.subs_) { + if (is_entity_local(sub_entity)) { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_SUBSCRIPTION_MATCHED, + 1); + local_entities_with_events[sub_entity].insert(ZENOH_EVENT_SUBSCRIPTION_MATCHED); + } + } + // Update event counters for the new entity. + if (is_entity_local(entity) && match_count_for_entity > 0) { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_PUBLICATION_MATCHED, + match_count_for_entity); + local_entities_with_events[entity].insert(ZENOH_EVENT_PUBLICATION_MATCHED); + } + } else { + // Entity is a sub. + // Count the number of matching pubs for each set of qos settings. + if (!topic_data_ptr->stats_.pubs_.empty()) { + match_count_for_entity += topic_data_ptr->stats_.pubs_.size(); + } + // Also iterate through the pubs to check if any are local and if update event counters. + for (const liveliness::Entity & pub_entity : topic_data_ptr->stats_.pubs_) { + if (is_entity_local(pub_entity)) { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_PUBLICATION_MATCHED, + 1); + local_entities_with_events[pub_entity].insert(ZENOH_EVENT_PUBLICATION_MATCHED); + } + } + // Update event counters for the new entity. + if (is_entity_local(entity) && match_count_for_entity > 0) { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_SUBSCRIPTION_MATCHED, + match_count_for_entity); + local_entities_with_events[entity].insert(ZENOH_EVENT_SUBSCRIPTION_MATCHED); + } + } + } + } + // We check if an entity with the exact same qos also exists. + GraphNode::TopicQoSMap::iterator topic_qos_map_it = topic_type_map_it->second.find( qos_str); - if (topic_qos_map_it == topic_data_map_it->second.end()) { + if (topic_qos_map_it == topic_type_map_it->second.end()) { // First time this qos is added. - topic_data_map_it->second.insert(std::make_pair(qos_str, graph_topic_data)); + // Update cache. + topic_type_map_it->second.insert(std::make_pair(qos_str, graph_topic_data)); } else { // We have another instance of a pub/sub over the same topic, // type and qos so we increment the counters. TopicDataPtr & existing_graph_topic = topic_qos_map_it->second; - existing_graph_topic->stats_.pub_count_ += pub_count; - existing_graph_topic->stats_.sub_count_ += sub_count; + if (is_pub) { + existing_graph_topic->stats_.pubs_.insert(entity); + } else { + existing_graph_topic->stats_.subs_.insert(entity); + } + } + } + } + // Take events if any. + if (report_events) { + take_local_entities_with_events(local_entities_with_events); + } +} + +///============================================================================= +void GraphCache::take_local_entities_with_events( + std::unordered_map> & + local_entities_with_events) +{ + if (local_entities_with_events.empty()) { + return; + } + + for (const auto & [local_entity, event_set] : local_entities_with_events) { + // Trigger callback set for this entity for the event type. + GraphEventCallbackMap::const_iterator event_callbacks_it = + event_callbacks_.find(local_entity); + if (event_callbacks_it != event_callbacks_.end()) { + for (const rmw_zenoh_event_type_t & event_type : event_set) { + GraphEventCallbacks::const_iterator callback_it = + event_callbacks_it->second.find(event_type); + if (callback_it != event_callbacks_it->second.end()) { + std::unique_ptr taken_event = + take_event_status(local_entity.topic_info()->name_, event_type); + callback_it->second(std::move(taken_event)); + } } } } @@ -267,87 +385,135 @@ void GraphCache::update_topic_maps_for_del( // Nothing to update for a node entity. return; } - - if (!entity.topic_info().has_value()) { - // This should not happen as add_topic_data() is called after validating the existence - // of topic_info. - RCUTILS_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "update_topic_maps_for_del() called for non-node entity without valid TopicInfo. " - "Report this."); - return; - } - - const liveliness::TopicInfo topic_info = entity.topic_info().value(); - // For the sake of reusing data structures and lookup functions, we treat publishers and - // clients are equivalent. Similarly, subscriptions and services are equivalent. - const std::size_t pub_count = entity.type() == EntityType::Publisher || - entity.type() == EntityType::Client ? 1 : 0; - const std::size_t sub_count = !pub_count; - // First update the topic map within the node. if (entity.type() == EntityType::Publisher) { - update_topic_map_for_del(graph_node->pubs_, topic_info, pub_count, sub_count); + update_topic_map_for_del(graph_node->pubs_, entity); } else if (entity.type() == EntityType::Subscription) { - update_topic_map_for_del(graph_node->subs_, topic_info, pub_count, sub_count); + update_topic_map_for_del(graph_node->subs_, entity); } else if (entity.type() == EntityType::Service) { - update_topic_map_for_del(graph_node->services_, topic_info, pub_count, sub_count); + update_topic_map_for_del(graph_node->services_, entity); } else { - update_topic_map_for_del(graph_node->clients_, topic_info, pub_count, sub_count); + update_topic_map_for_del(graph_node->clients_, entity); } // Then update the variables tracking topics across the graph. - // TODO(Yadunund): Check for QoS events. if (entity.type() == EntityType::Publisher || entity.type() == EntityType::Subscription) { - update_topic_map_for_del(this->graph_topics_, topic_info, pub_count, sub_count); + update_topic_map_for_del(this->graph_topics_, entity, true); } else { - update_topic_map_for_del(this->graph_services_, topic_info, pub_count, sub_count); + update_topic_map_for_del(this->graph_services_, entity); } } ///============================================================================= void GraphCache::update_topic_map_for_del( GraphNode::TopicMap & topic_map, - const liveliness::TopicInfo & topic_info, - const std::size_t pub_count, - const std::size_t sub_count) + const liveliness::Entity & entity, + bool report_events) { + if (!entity.topic_info().has_value()) { + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "update_topic_maps_for_del() called for non-node entity without valid TopicInfo. " + "Report this."); + return; + } + const liveliness::TopicInfo topic_info = entity.topic_info().value(); + const bool is_pub = is_entity_pub(entity); + // Initialize a map that will be populated with any QoS events that may be detected. + std::unordered_map> + local_entities_with_events = {}; + GraphNode::TopicMap::iterator cache_topic_it = topic_map.find(topic_info.name_); if (cache_topic_it == topic_map.end()) { // This should not happen. RCUTILS_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "topic_key %s not found in topic_map. Report this.", + "rmw_zenoh_cpp", "topic name %s not found in topic_map. Report this.", topic_info.name_.c_str()); + return; } else { - GraphNode::TopicTypeMap::iterator cache_topic_data_it = + GraphNode::TopicTypeMap::iterator cache_topic_type_it = cache_topic_it->second.find(topic_info.type_); - if (cache_topic_data_it != cache_topic_it->second.end()) { + if (cache_topic_type_it == cache_topic_it->second.end()) { + // This should not happen. + RCUTILS_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "topic type %s not found in for topic %s. Report this.", + topic_info.type_.c_str(), topic_info.name_.c_str()); + return; + } else { const std::string qos_str = liveliness::qos_to_keyexpr(topic_info.qos_); - GraphNode::TopicQoSMap::iterator cache_topic_qos_it = cache_topic_data_it->second.find( + GraphNode::TopicQoSMap::iterator cache_topic_qos_it = cache_topic_type_it->second.find( qos_str); - if (cache_topic_qos_it != cache_topic_data_it->second.end()) { - // Decrement the relevant counters. If both counters are 0 remove from cache. - cache_topic_qos_it->second->stats_.pub_count_ -= pub_count; - cache_topic_qos_it->second->stats_.sub_count_ -= sub_count; - if (cache_topic_qos_it->second->stats_.pub_count_ <= 0 && - cache_topic_qos_it->second->stats_.sub_count_ <= 0) - { - cache_topic_data_it->second.erase(qos_str); - } - // If the qos map is empty, erase it from the topic_data_map. - if (cache_topic_data_it->second.empty()) { - cache_topic_it->second.erase(cache_topic_data_it); + if (cache_topic_qos_it == cache_topic_type_it->second.end()) { + // This should not happen. + RCUTILS_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "qos %s not found in for topic type %s. Report this.", + qos_str.c_str(), topic_info.type_.c_str()); + return; + } + // Decrement the relevant counters. If both counters are 0 remove from cache. + if (is_pub) { + cache_topic_qos_it->second->stats_.pubs_.erase(entity); + } else { + cache_topic_qos_it->second->stats_.subs_.erase(entity); + } + // If after removing the entity, the parent map is empty, then remove parent + // map. + if (cache_topic_qos_it->second->stats_.pubs_.empty() && + cache_topic_qos_it->second->stats_.subs_.empty()) + { + cache_topic_type_it->second.erase(qos_str); + } + // Check for matched events + if (report_events) { + // TODO(Yadunund): Refactor into lambdas to reduce code duplication. + // We do not have to report any events for the entity removed event + // as it is already destructed. So we only check for matched entities + // in the graph that may be local. + if (is_pub) { + // Notify any local subs of a matched event with change -1. + for (const auto & [_, topic_data_ptr] : cache_topic_type_it->second) { + for (const liveliness::Entity & sub_entity : topic_data_ptr->stats_.subs_) { + if (is_entity_local(sub_entity)) { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_SUBSCRIPTION_MATCHED, + -1); + local_entities_with_events[sub_entity].insert(ZENOH_EVENT_SUBSCRIPTION_MATCHED); + } + } + } + } else { + // Notify any local pubs of a matched event with change -1. + for (const auto & [_, topic_data_ptr] : cache_topic_type_it->second) { + for (const liveliness::Entity & pub_entity : topic_data_ptr->stats_.pubs_) { + if (is_entity_local(pub_entity)) { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_PUBLICATION_MATCHED, + -1); + local_entities_with_events[pub_entity].insert(ZENOH_EVENT_PUBLICATION_MATCHED); + } + } + } } } + // If the type does not have any qos entries, erase it from the type map. + if (cache_topic_type_it->second.empty()) { + cache_topic_it->second.erase(cache_topic_type_it); + } // If the topic does not have any TopicData entries, erase the topic from the map. if (cache_topic_it->second.empty()) { topic_map.erase(cache_topic_it); } } } + // Take events if any. + if (report_events) { + take_local_entities_with_events(local_entities_with_events); + } } ///============================================================================= @@ -365,11 +531,21 @@ void GraphCache::remove_topic_map_from_cache( topic_type_it->second.begin(); topic_qos_it != topic_type_it->second.end(); ++topic_qos_it) { - update_topic_map_for_del( - from_cache, - topic_qos_it->second->info_, - topic_qos_it->second->stats_.pub_count_, - topic_qos_it->second->stats_.sub_count_); + // Technically only one of pubs_ or sub_ will be populated and with one + // element at most since to_remove comes from a node. For completeness, + // we iterate though both and call update_topic_map_for_del(). + for (const liveliness::Entity & entity : topic_qos_it->second->stats_.pubs_) { + update_topic_map_for_del( + from_cache, + entity, + true); + } + for (const liveliness::Entity & entity : topic_qos_it->second->stats_.subs_) { + update_topic_map_for_del( + from_cache, + entity, + true); + } } } } @@ -675,7 +851,7 @@ rmw_ret_t GraphCache::publisher_count_matched_subscriptions( if (topic_data_it != topic_it->second.end()) { for (const auto & [_, topic_data] : topic_data_it->second) { // If a subscription exists with compatible QoS, update the subscription count. - if (topic_data->stats_.sub_count_ > 0) { + if (!topic_data->stats_.subs_.empty()) { rmw_qos_compatibility_type_t is_compatible; rmw_ret_t ret = rmw_qos_profile_check_compatible( pub_data->adapted_qos_profile, @@ -684,7 +860,7 @@ rmw_ret_t GraphCache::publisher_count_matched_subscriptions( nullptr, 0); if (ret == RMW_RET_OK && is_compatible == RMW_QOS_COMPATIBILITY_OK) { - *subscription_count = *subscription_count + topic_data->stats_.sub_count_; + *subscription_count = *subscription_count + topic_data->stats_.subs_.size(); } } } @@ -710,7 +886,7 @@ rmw_ret_t GraphCache::subscription_count_matched_publishers( if (topic_data_it != topic_it->second.end()) { for (const auto & [_, topic_data] : topic_data_it->second) { // If a subscription exists with compatible QoS, update the subscription count. - if (topic_data->stats_.pub_count_ > 0) { + if (!topic_data->stats_.pubs_.empty()) { rmw_qos_compatibility_type_t is_compatible; rmw_ret_t ret = rmw_qos_profile_check_compatible( sub_data->adapted_qos_profile, @@ -719,7 +895,7 @@ rmw_ret_t GraphCache::subscription_count_matched_publishers( nullptr, 0); if (ret == RMW_RET_OK && is_compatible == RMW_QOS_COMPATIBILITY_OK) { - *publisher_count = *publisher_count + topic_data->stats_.pub_count_; + *publisher_count = *publisher_count + topic_data->stats_.pubs_.size(); } } } @@ -754,7 +930,7 @@ rmw_ret_t GraphCache::count_publishers( GraphNode::TopicQoSMap> & topic_data : graph_topics_.at(topic_name)) { for (auto it = topic_data.second.begin(); it != topic_data.second.end(); ++it) { - *count += it->second->stats_.pub_count_; + *count += it->second->stats_.pubs_.size(); } } } @@ -775,7 +951,7 @@ rmw_ret_t GraphCache::count_subscriptions( GraphNode::TopicQoSMap> & topic_data : graph_topics_.at(topic_name)) { for (auto it = topic_data.second.begin(); it != topic_data.second.end(); ++it) { - *count += it->second->stats_.sub_count_; + *count += it->second->stats_.subs_.size(); } } } @@ -796,7 +972,7 @@ rmw_ret_t GraphCache::count_services( GraphNode::TopicQoSMap> & topic_data : graph_services_.at(service_name)) { for (auto it = topic_data.second.begin(); it != topic_data.second.end(); ++it) { - *count += it->second->stats_.sub_count_; + *count += it->second->stats_.subs_.size(); } } } @@ -817,7 +993,7 @@ rmw_ret_t GraphCache::count_clients( GraphNode::TopicQoSMap> & topic_data : graph_services_.at(service_name)) { for (auto it = topic_data.second.begin(); it != topic_data.second.end(); ++it) { - *count += it->second->stats_.pub_count_; + *count += it->second->stats_.pubs_.size(); } } } @@ -1026,7 +1202,7 @@ rmw_ret_t GraphCache::service_server_is_available( GraphNode::TopicTypeMap::iterator type_it = service_it->second.find(service_type); if (type_it != service_it->second.end()) { for (const auto & [_, topic_data] : type_it->second) { - if (topic_data->stats_.sub_count_ > 0) { + if (topic_data->stats_.subs_.size() > 0) { *is_available = true; return RMW_RET_OK; } @@ -1046,25 +1222,87 @@ void GraphCache::set_qos_event_callback( std::lock_guard lock(graph_mutex_); if (event_type > ZENOH_EVENT_ID_MAX) { + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "set_qos_event_callback() called for unsupported event. Report this."); return; } - const std::string entity_key = entity.keyexpr(); - const GraphEventCallbackMap::iterator event_cb_it = event_callbacks_.find(entity_key); + const GraphEventCallbackMap::iterator event_cb_it = event_callbacks_.find(entity); if (event_cb_it == event_callbacks_.end()) { // First time a callback is being set for this entity. - event_callbacks_[entity_key] = {}; - event_callbacks_[entity_key].insert(std::make_pair(event_type, std::move(callback))); + event_callbacks_[entity] = {}; + event_callbacks_[entity].insert(std::make_pair(event_type, std::move(callback))); return; } - event_cb_it->second.insert(std::make_pair(event_type, std::move(callback))); + event_cb_it->second[event_type] = std::move(callback); + // printf( + // "[graph_cache] Set callback for rmw_zenoh_event_type_t %s for entity %s\n", + // std::to_string(event_type).c_str(), entity.keyexpr().c_str()); } ///============================================================================= -bool GraphCache::is_local_entity(const liveliness::Entity & entity) const +bool GraphCache::is_entity_local(const liveliness::Entity & entity) const { // For now zenoh does not expose unique IDs for its entities and hence the id // assigned to an entity is always the zenoh session id. When we update liveliness // tokens to contain globally unique ids for entities, we should also update the logic here. return entity.id() == zid_str_; } + +///============================================================================= +bool GraphCache::is_entity_pub(const liveliness::Entity & entity) const +{ + if (entity.type() == EntityType::Publisher || + entity.type() == EntityType::Client) + { + return true; + } + return false; +} + +///============================================================================= +void GraphCache::update_event_counters( + const std::string & topic_name, + const rmw_zenoh_event_type_t event_id, + int32_t change) +{ + if (event_id > ZENOH_EVENT_ID_MAX) { + return; + } + + auto event_statuses_it = event_statuses_.find(topic_name); + if (event_statuses_it == event_statuses_.end()) { + // Initialize statuses. + std::array status_array; + event_statuses_[topic_name] = std::move(status_array); + } + + rmw_zenoh_event_status_t & status_to_update = event_statuses_[topic_name][event_id]; + status_to_update.total_count += std::abs(change); + status_to_update.total_count_change += std::abs(change); + status_to_update.current_count += change; + status_to_update.current_count_change = change; +} + +///============================================================================= +std::unique_ptr GraphCache::take_event_status( + const std::string & topic_name, + const rmw_zenoh_event_type_t event_id) +{ + if (event_id > ZENOH_EVENT_ID_MAX) { + return nullptr; + } + + auto event_statuses_it = event_statuses_.find(topic_name); + if (event_statuses_it == event_statuses_.end()) { + return nullptr; + } + + rmw_zenoh_event_status_t & status_to_take = event_statuses_[topic_name][event_id]; + auto result = std::make_unique(status_to_take); + // Reset changes. + status_to_take.total_count_change = 0; + status_to_take.current_count_change = 0; + return result; +} diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 16b97003..17bdc7fc 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -38,19 +38,20 @@ ///============================================================================= // TODO(Yadunund): Since we reuse pub_count_ and sub_count_ for pub/sub and // service/client consider more general names for these fields. +// Consider changing this to an array of unordered_maps where the index of the +// array corresponds to the EntityType enum. This way we don't need to mix +// pub/sub with client/service. struct TopicStats { - // The count of publishers or clients. - std::size_t pub_count_; + // The guids of publishers or clients. + std::unordered_set pubs_; - // The count of subscriptions or services. - std::size_t sub_count_; - - // Constructor which initializes counters to 0. - TopicStats(std::size_t pub_count, std::size_t sub_count); + // The guids of subscriptions or services. + std::unordered_set subs_; }; ///============================================================================= +// TODO(Yadunund): Simplify to directly store pubs_ and subs_. struct TopicData { liveliness::TopicInfo info_; @@ -173,9 +174,6 @@ class GraphCache final const rmw_zenoh_event_type_t & event_type, GraphCacheEventCallback callback); - /// Returns true if the entity was created within the same context / zenoh session. - bool is_local_entity(const liveliness::Entity & entity) const; - private: // Helper function to convert an Entity into a GraphNode. // Note: this will update bookkeeping variables in GraphCache. @@ -188,9 +186,8 @@ class GraphCache final void update_topic_map_for_put( GraphNode::TopicMap & topic_map, - const liveliness::TopicInfo & topic_info, - const std::size_t pub_count, - const std::size_t sub_count); + const liveliness::Entity & entity, + bool report_events = false); void update_topic_maps_for_del( GraphNodePtr graph_node, @@ -198,14 +195,34 @@ class GraphCache final void update_topic_map_for_del( GraphNode::TopicMap & topic_map, - const liveliness::TopicInfo & topic_info, - const std::size_t pub_count, - const std::size_t sub_count); + const liveliness::Entity & entity, + bool report_events = false); void remove_topic_map_from_cache( const GraphNode::TopicMap & to_remove, GraphNode::TopicMap & from_cache); + /// Returns true if the entity was created within the same context / zenoh session. + bool is_entity_local(const liveliness::Entity & entity) const; + + /// Returns true if the entity is a publisher or client. False otherwise. + bool is_entity_pub(const liveliness::Entity & entity) const; + + void update_event_counters( + const std::string & topic_name, + const rmw_zenoh_event_type_t event_id, + int32_t change); + + // Take status and reset change counters. + std::unique_ptr take_event_status( + const std::string & topic_name, + const rmw_zenoh_event_type_t event_id); + + void take_local_entities_with_events( + std::unordered_map> & + local_entities_with_events); + + std::string zid_str_; /* namespace_1: @@ -242,15 +259,18 @@ class GraphCache final GraphNode::TopicMap graph_services_ = {}; using GraphEventCallbacks = std::unordered_map; - // Map the liveliness token of an entity to a map of event callbacks. + // Map entity (based on uuid) to a map of event callbacks. // Note: Since we use unordered_map, we will only store a single callback for an // entity string. So we do not support the case where a node create a duplicate // pub/sub with the exact same topic, type & QoS but registers a different callback // for the same event type. We could switch to a multimap here but removing the callback // will be impossible right now since entities do not have unique IDs. - using GraphEventCallbackMap = std::unordered_map; + using GraphEventCallbackMap = std::unordered_map; // EventCallbackMap for each type of event we support in rmw_zenoh_cpp. GraphEventCallbackMap event_callbacks_; + // Counters to track changes to event statues for each topic. + std::unordered_map> event_statuses_; // Mutex to lock before modifying the members above. mutable std::mutex graph_mutex_; diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index f8c2daa8..e67058e0 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -14,6 +14,7 @@ #include "liveliness_utils.hpp" +#include #include #include #include @@ -280,6 +281,8 @@ Entity::Entity( } this->keyexpr_ = token_ss.str(); + // TODO(Yadunund): Replace once zenoh api provides guids. + this->guid_ = std::hash{}(this->keyexpr_); } ///============================================================================= @@ -388,6 +391,12 @@ std::string Entity::id() const return this->id_; } +///============================================================================= +std::size_t Entity::guid() const +{ + return this->guid_; +} + ///============================================================================= EntityType Entity::type() const { @@ -421,6 +430,12 @@ std::string Entity::keyexpr() const return this->keyexpr_; } +///============================================================================= +bool Entity::operator==(const Entity & other) const +{ + return other.guid() == guid_; +} + ///============================================================================= std::string mangle_name(const std::string & input) { diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp index a0b1fa80..21759c74 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -76,9 +77,18 @@ enum class EntityType : uint8_t class Entity { public: - /// Make an Entity from datatypes. This will return nullopt if the required - /// fields are not present for the EntityType. // TODO(Yadunund): Find a way to better bundle the type and the associated data. + // Also make id globally unique when zenoh supports this. + // In the meanwhile, we rely on the interim _guid() method to return a hash value + // of the keyexpr which we use as guid. + + /// @brief Make an Entity from datatypes. This will return nullopt if the required + /// fields are not present for the EntityType. + /// @param id The zenoh + /// @param type + /// @param node_info + /// @param topic_info + /// @return static std::optional make( z_id_t id, EntityType type, @@ -88,8 +98,16 @@ class Entity /// Make an Entity from a liveliness keyexpr. static std::optional make(const std::string & keyexpr); + // Get the zenoh session id as a string. This is not unique as entities + // created within the same session, will have the same ids. std::string id() const; + // Interim method to get a "unique" id for this entity which is the hash of the keyexpr. + // Note: This is still not globally unique since two entities created with the + // same session with the same EntityType, topic name, topic type, and topic qos + // will have the same _id. Eg. If a node creates two publishers with same properties. + std::size_t guid() const; + /// Get the entity type. EntityType type() const; @@ -105,6 +123,9 @@ class Entity /// Get the liveliness keyexpr for this entity. std::string keyexpr() const; + // Two entities are equal if their guids are equal. + bool operator==(const Entity & other) const; + private: Entity( std::string id, @@ -113,6 +134,7 @@ class Entity std::optional topic_info); std::string id_; + std::size_t guid_; EntityType type_; NodeInfo node_info_; std::optional topic_info_; @@ -153,4 +175,18 @@ std::string zid_to_str(const z_id_t & id); } // namespace liveliness +///============================================================================= +// Allow Entity to be hashed and used as a key in unordered_maps/sets +namespace std +{ +template<> +struct hash +{ + auto operator()(const liveliness::Entity & entity) const -> size_t + { + return entity.guid(); + } +}; +} // namespace std + #endif // DETAIL__LIVELINESS_UTILS_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index b0c204c3..99654ec8 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -65,16 +65,17 @@ rmw_publisher_event_init( event_id = rmw_event_it->second](std::unique_ptr zenoh_event) { if (pub_data == nullptr) { - printf("CANNOT LOCK PUB DATA!!!\n"); return; } - printf("Successfully added event %zu from graph cache to event queue\n"); pub_data->add_new_event( event_id, std::move(zenoh_event)); } ); + // printf( + // "[rmw_publisher_event_init] created new rmw_event_type_t %s for %s\n", + // std::to_string(event_type).c_str(), pub_data->entity->keyexpr().c_str()); return RMW_RET_OK; } @@ -124,16 +125,17 @@ rmw_subscription_event_init( event_id = rmw_event_it->second](std::unique_ptr zenoh_event) { if (sub_data == nullptr) { - printf("CANNOT LOCK SUB DATA!!!\n"); return; } - printf("Successfully added event %zu from graph cache to event queue\n"); sub_data->add_new_event( event_id, std::move(zenoh_event)); } ); + // printf( + // "[rmw_subscription_event_init] created new rmw_event_type_t %s for %s\n", + // std::to_string(event_type).c_str(), sub_data->entity->keyexpr().c_str()); return RMW_RET_OK; } From 360cc8fca99ee9098c5f3182a9d3714ba7668a45 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sat, 24 Feb 2024 02:41:49 +0800 Subject: [PATCH 07/12] Make liveliness token keyexprs globally unique Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 42 ++--- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 2 +- rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 143 ++++++++++-------- rmw_zenoh_cpp/src/detail/liveliness_utils.hpp | 64 +++++--- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 5 + rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 9 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 5 + 7 files changed, 162 insertions(+), 108 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 5e90e38a..ce7874b6 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -58,14 +58,8 @@ GraphCache::GraphCache(const z_id_t & zid) ///============================================================================= std::shared_ptr GraphCache::make_graph_node(const Entity & entity) const { - if (entity.type() == EntityType::Invalid) { - RCUTILS_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "make_graph_node() called for Invalid entity. Report this."); - return nullptr; - } auto graph_node = std::make_shared(); - graph_node->id_ = entity.id(); + graph_node->zid_ = entity.zid(); graph_node->ns_ = entity.node_namespace(); graph_node->name_ = entity.node_name(); graph_node->enclave_ = entity.node_enclave(); @@ -309,13 +303,6 @@ void GraphCache::parse_put(const std::string & keyexpr) const liveliness::Entity & entity = *maybe_entity; - if (entity.type() == EntityType::Invalid) { - RCUTILS_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "parse_put() called for an invalid entity. Report this."); - return; - } - // Lock the graph mutex before accessing the graph. std::lock_guard lock(graph_mutex_); @@ -346,7 +333,14 @@ void GraphCache::parse_put(const std::string & keyexpr) range.first, range.second, [&entity](const std::pair & node_it) { - return entity.id() == node_it.second->id_; + // TODO(Yadunund): We need to encode the node's id in every entity's liveliness + // token. Right now we cannot differentiate between two nodes from the same session + // with the same name and namespace. + const bool match = + entity.zid() == node_it.second->zid_ && + entity.node_namespace() == node_it.second->ns_ && + entity.node_name() == node_it.second->name_; + return match; }); if (node_it == range.second) { // Either the first time a node with this name is added or with an existing @@ -363,10 +357,9 @@ void GraphCache::parse_put(const std::string & keyexpr) if (insertion_it == ns_it->second.end()) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", - "Unable to add a new node /%s with id %s an " + "Unable to add a new node /%s to an " "existing namespace %s in the graph. Report this bug.", entity.node_name().c_str(), - entity.id().c_str(), entity.node_namespace().c_str()); } return; @@ -561,13 +554,6 @@ void GraphCache::parse_del(const std::string & keyexpr) } const liveliness::Entity entity = *maybe_entity; - if (entity.type() == EntityType::Invalid) { - RCUTILS_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "parse_del() called for an invalid entity. Report this."); - return; - } - // Lock the graph mutex before accessing the graph. std::lock_guard lock(graph_mutex_); @@ -584,7 +570,11 @@ void GraphCache::parse_del(const std::string & keyexpr) range.first, range.second, [&entity](const std::pair & node_it) { - return entity.id() == node_it.second->id_; + const bool match = + entity.zid() == node_it.second->zid_ && + entity.node_namespace() == node_it.second->ns_ && + entity.node_name() == node_it.second->name_; + return match; }); if (node_it == range.second) { // Node does not exist. @@ -1247,7 +1237,7 @@ bool GraphCache::is_entity_local(const liveliness::Entity & entity) const // For now zenoh does not expose unique IDs for its entities and hence the id // assigned to an entity is always the zenoh session id. When we update liveliness // tokens to contain globally unique ids for entities, we should also update the logic here. - return entity.id() == zid_str_; + return entity.zid() == zid_str_; } ///============================================================================= diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 17bdc7fc..afbcd2a5 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -66,7 +66,7 @@ using TopicDataPtr = std::shared_ptr; ///============================================================================= struct GraphNode { - std::string id_; + std::string zid_; std::string ns_; std::string name_; // TODO(Yadunund): Should enclave be the parent to the namespace key and not within a Node? diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index e67058e0..65c743cc 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -61,6 +61,24 @@ TopicInfo::TopicInfo( ///============================================================================= namespace { +/// Enum of liveliness key-expression components. +enum KeyexprIndex +{ + AdminSpace, + DomainId, + Zid, + Id, + EntityStr, + Namespace, + NodeName, + TopicName, + TopicType, + TopicQoS +}; + +// Every keyexpression will have components upto node name. +#define KEYEXPR_INDEX_MIN KeyexprIndex::NodeName +#define KEYEXPR_INDEX_MAX KeyexprIndex::TopicQoS /// The admin space used to prefix the liveliness tokens. static const char ADMIN_SPACE[] = "@ros2_lv"; @@ -69,6 +87,8 @@ static const char PUB_STR[] = "MP"; static const char SUB_STR[] = "MS"; static const char SRV_STR[] = "SS"; static const char CLI_STR[] = "SC"; +static const char EMPTY_NAMESPACE_REPLACEMENT = '_'; +static const char KEYEXPR_DELIMITER = '/'; static const char SLASH_REPLACEMENT = '%'; static const char QOS_DELIMITER = ':'; static const char QOS_HISTORY_DELIMITER = ','; @@ -116,7 +136,7 @@ static const std::unordered_map str_to std::vector split_keyexpr( const std::string & keyexpr, - const char delim = '/') + const char delim = KEYEXPR_DELIMITER) { std::vector result = {}; size_t start = 0; @@ -219,79 +239,65 @@ std::string subscription_token(size_t domain_id) ///============================================================================= Entity::Entity( + std::string zid, std::string id, EntityType type, NodeInfo node_info, std::optional topic_info) -: id_(std::move(id)), +: zid_(std::move(zid)), + id_(std::move(id)), type_(std::move(type)), node_info_(std::move(node_info)), topic_info_(std::move(topic_info)) { - /** - * Set the liveliness token for the particular entity. - * - * The liveliness token keyexprs are in the form: - * - * ///// - * - * Where: - * - A number set by the user to "partition" graphs. Roughly equivalent to the domain ID in DDS. - * - A unique ID to identify this entity. Currently the id is the zenoh session's id with elements concatenated into a string using '.' as separator. - * - The type of entity. This can be one of "NN" for a node, "MP" for a publisher, "MS" for a subscription, "SS" for a service server, or "SC" for a service client. - * - The ROS namespace for this entity. If the namespace is absolute, this function will add in an _ for later parsing reasons. - * - The ROS node name for this entity. - * - * For entities with topic infomation, the liveliness token keyexpr have additional fields: - * - * //////// - * - The ROS topic name for this entity. - * - The type for the topic. - * - The qos for the topic (see qos_to_keyexpr() docstring for more information). - * - * For example, the liveliness expression for a publisher within a /talker node that publishes - * an std_msgs/msg/String over topic /chatter and with QoS settings of Reliability: best_effort, - * Durability: transient_local, History: keep_all, and depth: 10, would be - * "@ros2_lv/0/q1w2e3r4t5y6/MP/_/talker/dds_::std_msgs::msg::String/2:1:2,10". - * Note: The domain_id is assumed to be 0 and a random id is used in the example. Also the - * _dds:: prefix in the topic_type is an artifact of the type support implementation and is - * removed when reporting the topic_type in graph_cache.cpp (see _demangle_if_ros_type()). - */ - std::stringstream token_ss; - const std::string & ns = node_info_.ns_; - token_ss << ADMIN_SPACE << "/" << node_info_.domain_id_ << "/" << id_ << "/" << entity_to_str.at( - type_) << ns; + std::string keyexpr_parts[KEYEXPR_INDEX_MAX + 1] {}; + keyexpr_parts[KeyexprIndex::AdminSpace] = ADMIN_SPACE; + keyexpr_parts[KeyexprIndex::DomainId] = std::to_string(node_info_.domain_id_); + keyexpr_parts[KeyexprIndex::Zid] = zid_; + keyexpr_parts[KeyexprIndex::Id] = id_; + keyexpr_parts[KeyexprIndex::EntityStr] = entity_to_str.at(type_); // An empty namespace from rcl will contain "/" but zenoh does not allow keys with "//". // Hence we add an "_" to denote an empty namespace such that splitting the key // will always result in 5 parts. - if (ns == "/") { - token_ss << "_/"; - } else { - token_ss << "/"; - } - // Finally append node name. - token_ss << mangle_name(node_info_.name_); + keyexpr_parts[KeyexprIndex::Namespace] = mangle_name(node_info_.ns_); + keyexpr_parts[KeyexprIndex::NodeName] = mangle_name(node_info_.name_); // If this entity has a topic info, append it to the token. if (topic_info_.has_value()) { const auto & topic_info = this->topic_info_.value(); - // Note: We don't append a leading "/" as we expect the ROS topic name to start with a "/". - token_ss << - "/" + mangle_name(topic_info.name_) + "/" + topic_info.type_ + "/" + qos_to_keyexpr( - topic_info.qos_); + keyexpr_parts[KeyexprIndex::TopicName] = mangle_name(topic_info.name_); + keyexpr_parts[KeyexprIndex::TopicType] = mangle_name(topic_info.type_); + keyexpr_parts[KeyexprIndex::TopicQoS] = qos_to_keyexpr(topic_info.qos_); } - this->keyexpr_ = token_ss.str(); - // TODO(Yadunund): Replace once zenoh api provides guids. + for (std::size_t i = 0; i < KEYEXPR_INDEX_MAX + 1; ++i) { + bool last = false; + if (!keyexpr_parts[i].empty()) { + this->keyexpr_ += std::move(keyexpr_parts[i]); + } + if (i == KEYEXPR_INDEX_MAX || keyexpr_parts[i + 1].empty()) { + last = true; + } + if (last) { + break; + } + // Append the delimiter unless it is the last component. + this->keyexpr_ += KEYEXPR_DELIMITER; + } this->guid_ = std::hash{}(this->keyexpr_); } ///============================================================================= std::optional Entity::make( - z_id_t id, + z_id_t zid, + const std::string & id, EntityType type, NodeInfo node_info, std::optional topic_info) { + if (id.empty()) { + RCUTILS_SET_ERROR_MSG("Invalid id."); + return std::nullopt; + } if (entity_to_str.find(type) == entity_to_str.end()) { RCUTILS_SET_ERROR_MSG("Invalid entity type."); return std::nullopt; @@ -305,7 +311,8 @@ std::optional Entity::make( return std::nullopt; } - Entity entity{zid_to_str(id), std::move(type), std::move(node_info), std::move(topic_info)}; + Entity entity{zid_to_str(zid), std::move(id), std::move(type), std::move(node_info), std::move( + topic_info)}; return entity; } @@ -313,10 +320,10 @@ std::optional Entity::make( std::optional Entity::make(const std::string & keyexpr) { std::vector parts = split_keyexpr(keyexpr); - // A token will contain at least 5 parts: - // (ADMIN_SPACE, domain_id, entity_str, namespace, node_name). + // Every token will contain at least 7 parts: + // (ADMIN_SPACE, domain_id, zid, id, entity_type, namespace, node_name). // Basic validation. - if (parts.size() < 6) { + if (parts.size() < KEYEXPR_INDEX_MIN + 1) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Received invalid liveliness token"); @@ -331,7 +338,7 @@ std::optional Entity::make(const std::string & keyexpr) } } - if (parts[0] != ADMIN_SPACE) { + if (parts[KeyexprIndex::AdminSpace] != ADMIN_SPACE) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Received liveliness token with invalid admin space."); @@ -339,7 +346,7 @@ std::optional Entity::make(const std::string & keyexpr) } // Get the entity, ie NN, MP, MS, SS, SC. - std::string & entity_str = parts[3]; + std::string & entity_str = parts[KeyexprIndex::EntityStr]; std::unordered_map::const_iterator entity_it = str_to_entity.find(entity_str); if (entity_it == str_to_entity.end()) { @@ -350,21 +357,22 @@ std::optional Entity::make(const std::string & keyexpr) } EntityType entity_type = entity_it->second; - std::size_t domain_id = std::stoul(parts[1]); - std::string & id = parts[2]; - std::string ns = parts[4] == "_" ? "/" : "/" + std::move(parts[4]); - std::string node_name = demangle_name(std::move(parts[5])); + std::size_t domain_id = std::stoul(parts[KeyexprIndex::DomainId]); + std::string & zid = parts[KeyexprIndex::Zid]; + std::string & id = parts[KeyexprIndex::Id]; + std::string ns = demangle_name(std::move(parts[KeyexprIndex::Namespace])); + std::string node_name = demangle_name(std::move(parts[KeyexprIndex::NodeName])); std::optional topic_info = std::nullopt; // Populate topic_info if we have a token for an entity other than a node. if (entity_type != EntityType::Node) { - if (parts.size() < 9) { + if (parts.size() < KEYEXPR_INDEX_MAX + 1) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Received liveliness token for non-node entity without required parameters."); return std::nullopt; } - std::optional qos = keyexpr_to_qos(parts[8]); + std::optional qos = keyexpr_to_qos(parts[KeyexprIndex::TopicQoS]); if (!qos.has_value()) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -372,19 +380,26 @@ std::optional Entity::make(const std::string & keyexpr) return std::nullopt; } topic_info = TopicInfo{ - demangle_name(std::move(parts[6])), - std::move(parts[7]), + demangle_name(std::move(parts[KeyexprIndex::TopicName])), + demangle_name(std::move(parts[KeyexprIndex::TopicType])), std::move(qos.value()) }; } return Entity{ + std::move(zid), std::move(id), std::move(entity_type), NodeInfo{std::move(domain_id), std::move(ns), std::move(node_name), ""}, std::move(topic_info)}; } +///============================================================================= +std::string Entity::zid() const +{ + return this->zid_; +} + ///============================================================================= std::string Entity::id() const { @@ -433,6 +448,8 @@ std::string Entity::keyexpr() const ///============================================================================= bool Entity::operator==(const Entity & other) const { + // TODO(Yadunund): If we decide to directly store the guid as a + // rmw_gid_t type, we should rely on rmw_compare_gids_equal() instead. return other.guid() == guid_; } diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp index 21759c74..d7292d5a 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp @@ -62,7 +62,6 @@ std::string subscription_token(size_t domain_id); ///============================================================================= enum class EntityType : uint8_t { - Invalid = 0, Node, Publisher, Subscription, @@ -72,25 +71,51 @@ enum class EntityType : uint8_t ///============================================================================= // An struct to bundle results of parsing a token. -// TODO(Yadunund): Consider using variadic templates to pass args instead of -// relying on optional fields. +/** + * Every entity will generate a unique key-expression for setting up a liveliness token. + * + * The minimal key-expression is of the form: + * + * ////// + * + * Where: + * - A number set by the user to "partition" graphs. Roughly equivalent to the domain ID in DDS. + * - The zenoh session's id with elements concatenated into a string using '.' as separator. + * - A unique ID within the zenoh session to identify this entity. + * - The type of entity. This can be one of "NN" for a Network Node, "MP" for a Message Publisher, "MS" for a Message Subscription, "SS" for a Service Server, or "SC" for a Service Client. + * - The ROS namespace for this entity. If the namespace is absolute, this function will add in an _ for later parsing reasons. + * - The ROS node name for this entity. + * + * For entities with topic infomation, the liveliness token keyexpr have additional fields: + * + * ///////// + * - The ROS topic name for this entity. + * - The type for the topic. + * - The qos for the topic (see qos_to_keyexpr() docstring for more information). + * + * For example, the liveliness expression for a publisher within a /talker node that publishes + * an std_msgs/msg/String over topic /chatter and with QoS settings of Reliability: best_effort, + * Durability: transient_local, History: keep_all, and depth: 10, would be + * "@ros2_lv/0/q1w2e3r4t5y6/32/MP/_/talker/dds_::std_msgs::msg::String/2:1:2,10". + * Note: The domain_id is assumed to be 0 and a random id is used in the example. Also the + * _dds:: prefix in the topic_type is an artifact of the type support implementation and is + * removed when reporting the topic_type in graph_cache.cpp (see _demangle_if_ros_type()). + */ class Entity { public: // TODO(Yadunund): Find a way to better bundle the type and the associated data. - // Also make id globally unique when zenoh supports this. - // In the meanwhile, we rely on the interim _guid() method to return a hash value - // of the keyexpr which we use as guid. - /// @brief Make an Entity from datatypes. This will return nullopt if the required /// fields are not present for the EntityType. - /// @param id The zenoh - /// @param type - /// @param node_info - /// @param topic_info - /// @return + /// @param zid The zenoh session id within which this entity was created. + /// @param id A unique id for this entity within the zenoh session. + /// @param type The type of the entity. + /// @param node_info The node information that is required for all entities. + /// @param topic_info An optional topic information for relevant entities. + /// @return An entity if all inputs are valid. This way no invalid entities can be created. static std::optional make( - z_id_t id, + z_id_t zid, + const std::string & id, EntityType type, NodeInfo node_info, std::optional topic_info = std::nullopt); @@ -100,12 +125,15 @@ class Entity // Get the zenoh session id as a string. This is not unique as entities // created within the same session, will have the same ids. + std::string zid() const; + + // Get the id of the entity local to a zenoh session. + // Use guid() to retrieve a globally unique id. std::string id() const; - // Interim method to get a "unique" id for this entity which is the hash of the keyexpr. - // Note: This is still not globally unique since two entities created with the - // same session with the same EntityType, topic name, topic type, and topic qos - // will have the same _id. Eg. If a node creates two publishers with same properties. + // Interim method to get a globally unique id for this entity which is the hash of the keyexpr. + // TODO(Yadunund): Should this return a rmw_gid_t? + // This is named guid and not gid to remain distinct as it is not of type rmw_gid_t. std::size_t guid() const; /// Get the entity type. @@ -128,11 +156,13 @@ class Entity private: Entity( + std::string zid, std::string id, EntityType type, NodeInfo node_info, std::optional topic_info); + std::string zid_; std::string id_; std::size_t guid_; EntityType type_; diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 28023fb5..2037c80b 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -26,6 +26,11 @@ #include "rmw_data_types.hpp" +///============================================================================= +size_t rmw_context_impl_s::get_next_entity_id() +{ + return next_entity_id_++; +} ///============================================================================= saved_msg_data::saved_msg_data(zc_owned_payload_t p, uint64_t recv_ts, const uint8_t pub_gid[16]) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 40b05624..78a0e941 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -41,8 +41,9 @@ /// Structs for various type erased data fields. ///============================================================================= -struct rmw_context_impl_s +class rmw_context_impl_s { +public: // An owned session. z_owned_session_t session; @@ -60,6 +61,12 @@ struct rmw_context_impl_s rmw_guard_condition_t * graph_guard_condition; std::unique_ptr graph_cache; + + size_t get_next_entity_id(); + +private: + // A counter to assign a local id for every entity created in this session. + size_t next_entity_id_{0}; }; ///============================================================================= diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index d4bdf862..ebc97535 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -288,6 +288,7 @@ rmw_create_node( rmw_node_data_t * node_data = static_cast(node->data); const auto liveliness_entity = liveliness::Entity::make( z_info_zid(z_loan(context->impl->session)), + std::to_string(context->impl->get_next_entity_id()), liveliness::EntityType::Node, liveliness::NodeInfo{context->actual_domain_id, namespace_, name, ""}); if (!liveliness_entity.has_value()) { @@ -603,6 +604,7 @@ rmw_create_publisher( publisher_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), + std::to_string(context_impl->get_next_entity_id()), liveliness::EntityType::Publisher, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, liveliness::TopicInfo{rmw_publisher->topic_name, @@ -1346,6 +1348,7 @@ rmw_create_subscription( // Publish to the graph that a new subscription is in town sub_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), + std::to_string(context_impl->get_next_entity_id()), liveliness::EntityType::Subscription, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, liveliness::TopicInfo{rmw_subscription->topic_name, @@ -1988,6 +1991,7 @@ rmw_create_client( } client_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), + std::to_string(context_impl->get_next_entity_id()), liveliness::EntityType::Client, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, liveliness::TopicInfo{rmw_client->service_name, @@ -2645,6 +2649,7 @@ rmw_create_service( } service_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), + std::to_string(context_impl->get_next_entity_id()), liveliness::EntityType::Service, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, liveliness::TopicInfo{rmw_service->service_name, From e182420b1860d1551dfcadc530cf5c891f1e31b6 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 26 Feb 2024 13:19:32 +0800 Subject: [PATCH 08/12] Add node id to all liveliness tokens Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/event.hpp | 7 ++ rmw_zenoh_cpp/src/detail/graph_cache.cpp | 77 ++++++++++--------- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 2 + rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 22 +++++- rmw_zenoh_cpp/src/detail/liveliness_utils.hpp | 13 +++- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 4 + rmw_zenoh_cpp/src/rmw_zenoh.cpp | 36 ++++++++- 7 files changed, 114 insertions(+), 47 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/event.hpp b/rmw_zenoh_cpp/src/detail/event.hpp index be065041..0adc05cb 100644 --- a/rmw_zenoh_cpp/src/detail/event.hpp +++ b/rmw_zenoh_cpp/src/detail/event.hpp @@ -72,6 +72,13 @@ struct rmw_zenoh_event_status_t size_t current_count_change; // The data field can be used to store serialized information for more complex statuses. std::string data; + + rmw_zenoh_event_status_t() + : total_count(0), + total_count_change(0), + current_count(0), + current_count_change(0) + {} }; ///============================================================================= diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index ce7874b6..dec5ec38 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -60,6 +60,7 @@ std::shared_ptr GraphCache::make_graph_node(const Entity & entity) co { auto graph_node = std::make_shared(); graph_node->zid_ = entity.zid(); + graph_node->nid_ = entity.nid(); graph_node->ns_ = entity.node_namespace(); graph_node->name_ = entity.node_name(); graph_node->enclave_ = entity.node_enclave(); @@ -197,20 +198,20 @@ void GraphCache::update_topic_map_for_put( } // Also iterate through the subs to check if any are local and if update event counters. for (const liveliness::Entity & sub_entity : topic_data_ptr->stats_.subs_) { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_SUBSCRIPTION_MATCHED, + static_cast(1)); if (is_entity_local(sub_entity)) { - update_event_counters( - topic_info.name_, - ZENOH_EVENT_SUBSCRIPTION_MATCHED, - 1); local_entities_with_events[sub_entity].insert(ZENOH_EVENT_SUBSCRIPTION_MATCHED); } } // Update event counters for the new entity. + update_event_counters( + topic_info.name_, + ZENOH_EVENT_PUBLICATION_MATCHED, + match_count_for_entity); if (is_entity_local(entity) && match_count_for_entity > 0) { - update_event_counters( - topic_info.name_, - ZENOH_EVENT_PUBLICATION_MATCHED, - match_count_for_entity); local_entities_with_events[entity].insert(ZENOH_EVENT_PUBLICATION_MATCHED); } } else { @@ -221,20 +222,20 @@ void GraphCache::update_topic_map_for_put( } // Also iterate through the pubs to check if any are local and if update event counters. for (const liveliness::Entity & pub_entity : topic_data_ptr->stats_.pubs_) { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_PUBLICATION_MATCHED, + static_cast(1)); if (is_entity_local(pub_entity)) { - update_event_counters( - topic_info.name_, - ZENOH_EVENT_PUBLICATION_MATCHED, - 1); local_entities_with_events[pub_entity].insert(ZENOH_EVENT_PUBLICATION_MATCHED); } } // Update event counters for the new entity. + update_event_counters( + topic_info.name_, + ZENOH_EVENT_SUBSCRIPTION_MATCHED, + match_count_for_entity); if (is_entity_local(entity) && match_count_for_entity > 0) { - update_event_counters( - topic_info.name_, - ZENOH_EVENT_SUBSCRIPTION_MATCHED, - match_count_for_entity); local_entities_with_events[entity].insert(ZENOH_EVENT_SUBSCRIPTION_MATCHED); } } @@ -295,6 +296,7 @@ void GraphCache::take_local_entities_with_events( ///============================================================================= void GraphCache::parse_put(const std::string & keyexpr) { + printf("[parse_put %s] %s\n", zid_str_.c_str(), keyexpr.c_str()); std::optional maybe_entity = liveliness::Entity::make(keyexpr); if (!maybe_entity.has_value()) { // Error message has already been logged. @@ -333,14 +335,8 @@ void GraphCache::parse_put(const std::string & keyexpr) range.first, range.second, [&entity](const std::pair & node_it) { - // TODO(Yadunund): We need to encode the node's id in every entity's liveliness - // token. Right now we cannot differentiate between two nodes from the same session - // with the same name and namespace. - const bool match = - entity.zid() == node_it.second->zid_ && - entity.node_namespace() == node_it.second->ns_ && - entity.node_name() == node_it.second->name_; - return match; + // Match nodes if their zenoh sesion and node ids match. + return entity.zid() == node_it.second->zid_ && entity.nid() == node_it.second->nid_; }); if (node_it == range.second) { // Either the first time a node with this name is added or with an existing @@ -469,12 +465,15 @@ void GraphCache::update_topic_map_for_del( // Notify any local subs of a matched event with change -1. for (const auto & [_, topic_data_ptr] : cache_topic_type_it->second) { for (const liveliness::Entity & sub_entity : topic_data_ptr->stats_.subs_) { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_SUBSCRIPTION_MATCHED, + static_cast(-1)); if (is_entity_local(sub_entity)) { - update_event_counters( - topic_info.name_, - ZENOH_EVENT_SUBSCRIPTION_MATCHED, - -1); local_entities_with_events[sub_entity].insert(ZENOH_EVENT_SUBSCRIPTION_MATCHED); + printf( + "Updating matched count by -1 for local sub: %s\n", + sub_entity.keyexpr().c_str()); } } } @@ -482,11 +481,11 @@ void GraphCache::update_topic_map_for_del( // Notify any local pubs of a matched event with change -1. for (const auto & [_, topic_data_ptr] : cache_topic_type_it->second) { for (const liveliness::Entity & pub_entity : topic_data_ptr->stats_.pubs_) { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_PUBLICATION_MATCHED, + static_cast(-1)); if (is_entity_local(pub_entity)) { - update_event_counters( - topic_info.name_, - ZENOH_EVENT_PUBLICATION_MATCHED, - -1); local_entities_with_events[pub_entity].insert(ZENOH_EVENT_PUBLICATION_MATCHED); } } @@ -547,6 +546,7 @@ void GraphCache::remove_topic_map_from_cache( ///============================================================================= void GraphCache::parse_del(const std::string & keyexpr) { + printf("[parse_del %s] %s\n", zid_str_.c_str(), keyexpr.c_str()); std::optional maybe_entity = liveliness::Entity::make(keyexpr); if (!maybe_entity.has_value()) { // Error message has already been logged. @@ -570,11 +570,8 @@ void GraphCache::parse_del(const std::string & keyexpr) range.first, range.second, [&entity](const std::pair & node_it) { - const bool match = - entity.zid() == node_it.second->zid_ && - entity.node_namespace() == node_it.second->ns_ && - entity.node_name() == node_it.second->name_; - return match; + // Match nodes if their zenoh sesion and node ids match. + return entity.zid() == node_it.second->zid_ && entity.nid() == node_it.second->nid_; }); if (node_it == range.second) { // Node does not exist. @@ -1261,10 +1258,12 @@ void GraphCache::update_event_counters( return; } + std::lock_guard lock(events_mutex_); + auto event_statuses_it = event_statuses_.find(topic_name); if (event_statuses_it == event_statuses_.end()) { // Initialize statuses. - std::array status_array; + std::array status_array {}; event_statuses_[topic_name] = std::move(status_array); } @@ -1284,6 +1283,8 @@ std::unique_ptr GraphCache::take_event_status( return nullptr; } + std::lock_guard lock(events_mutex_); + auto event_statuses_it = event_statuses_.find(topic_name); if (event_statuses_it == event_statuses_.end()) { return nullptr; diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index afbcd2a5..2ebd363c 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -67,6 +67,7 @@ using TopicDataPtr = std::shared_ptr; struct GraphNode { std::string zid_; + std::string nid_; std::string ns_; std::string name_; // TODO(Yadunund): Should enclave be the parent to the namespace key and not within a Node? @@ -271,6 +272,7 @@ class GraphCache final // Counters to track changes to event statues for each topic. std::unordered_map> event_statuses_; + std::mutex events_mutex_; // Mutex to lock before modifying the members above. mutable std::mutex graph_mutex_; diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index 65c743cc..d7455130 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -67,6 +67,7 @@ enum KeyexprIndex AdminSpace, DomainId, Zid, + Nid, Id, EntityStr, Namespace, @@ -240,11 +241,13 @@ std::string subscription_token(size_t domain_id) ///============================================================================= Entity::Entity( std::string zid, + std::string nid, std::string id, EntityType type, NodeInfo node_info, std::optional topic_info) : zid_(std::move(zid)), + nid_(std::move(nid)), id_(std::move(id)), type_(std::move(type)), node_info_(std::move(node_info)), @@ -254,6 +257,7 @@ Entity::Entity( keyexpr_parts[KeyexprIndex::AdminSpace] = ADMIN_SPACE; keyexpr_parts[KeyexprIndex::DomainId] = std::to_string(node_info_.domain_id_); keyexpr_parts[KeyexprIndex::Zid] = zid_; + keyexpr_parts[KeyexprIndex::Nid] = nid_; keyexpr_parts[KeyexprIndex::Id] = id_; keyexpr_parts[KeyexprIndex::EntityStr] = entity_to_str.at(type_); // An empty namespace from rcl will contain "/" but zenoh does not allow keys with "//". @@ -289,6 +293,7 @@ Entity::Entity( ///============================================================================= std::optional Entity::make( z_id_t zid, + const std::string & nid, const std::string & id, EntityType type, NodeInfo node_info, @@ -311,8 +316,13 @@ std::optional Entity::make( return std::nullopt; } - Entity entity{zid_to_str(zid), std::move(id), std::move(type), std::move(node_info), std::move( - topic_info)}; + Entity entity{ + zid_to_str(zid), + std::move(nid), + std::move(id), + std::move(type), + std::move(node_info), + std::move(topic_info)}; return entity; } @@ -359,6 +369,7 @@ std::optional Entity::make(const std::string & keyexpr) EntityType entity_type = entity_it->second; std::size_t domain_id = std::stoul(parts[KeyexprIndex::DomainId]); std::string & zid = parts[KeyexprIndex::Zid]; + std::string & nid = parts[KeyexprIndex::Nid]; std::string & id = parts[KeyexprIndex::Id]; std::string ns = demangle_name(std::move(parts[KeyexprIndex::Namespace])); std::string node_name = demangle_name(std::move(parts[KeyexprIndex::NodeName])); @@ -388,6 +399,7 @@ std::optional Entity::make(const std::string & keyexpr) return Entity{ std::move(zid), + std::move(nid), std::move(id), std::move(entity_type), NodeInfo{std::move(domain_id), std::move(ns), std::move(node_name), ""}, @@ -400,6 +412,12 @@ std::string Entity::zid() const return this->zid_; } +///============================================================================= +std::string Entity::nid() const +{ + return this->nid_; +} + ///============================================================================= std::string Entity::id() const { diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp index d7292d5a..db498073 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp @@ -76,12 +76,13 @@ enum class EntityType : uint8_t * * The minimal key-expression is of the form: * - * ////// + * /////// * * Where: * - A number set by the user to "partition" graphs. Roughly equivalent to the domain ID in DDS. * - The zenoh session's id with elements concatenated into a string using '.' as separator. - * - A unique ID within the zenoh session to identify this entity. + * - A unique ID within the zenoh session of the node which created this entity. + * - A unique ID within the zenoh session to identify this entity. When entity is a node, the id and nid are equal. * - The type of entity. This can be one of "NN" for a Network Node, "MP" for a Message Publisher, "MS" for a Message Subscription, "SS" for a Service Server, or "SC" for a Service Client. * - The ROS namespace for this entity. If the namespace is absolute, this function will add in an _ for later parsing reasons. * - The ROS node name for this entity. @@ -96,7 +97,7 @@ enum class EntityType : uint8_t * For example, the liveliness expression for a publisher within a /talker node that publishes * an std_msgs/msg/String over topic /chatter and with QoS settings of Reliability: best_effort, * Durability: transient_local, History: keep_all, and depth: 10, would be - * "@ros2_lv/0/q1w2e3r4t5y6/32/MP/_/talker/dds_::std_msgs::msg::String/2:1:2,10". + * "@ros2_lv/0/q1w2e3r4t5y6/1/32/MP/_/talker/dds_::std_msgs::msg::String/2:1:2,10". * Note: The domain_id is assumed to be 0 and a random id is used in the example. Also the * _dds:: prefix in the topic_type is an artifact of the type support implementation and is * removed when reporting the topic_type in graph_cache.cpp (see _demangle_if_ros_type()). @@ -115,6 +116,7 @@ class Entity /// @return An entity if all inputs are valid. This way no invalid entities can be created. static std::optional make( z_id_t zid, + const std::string & nid, const std::string & id, EntityType type, NodeInfo node_info, @@ -127,6 +129,9 @@ class Entity // created within the same session, will have the same ids. std::string zid() const; + // Get the id of the node of this entity. + std::string nid() const; + // Get the id of the entity local to a zenoh session. // Use guid() to retrieve a globally unique id. std::string id() const; @@ -157,12 +162,14 @@ class Entity private: Entity( std::string zid, + std::string nid, std::string id, EntityType type, NodeInfo node_info, std::optional topic_info); std::string zid_; + std::string nid_; std::string id_; std::size_t guid_; EntityType type_; diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 78a0e941..01a9b9ce 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -76,6 +76,10 @@ struct rmw_node_data_t // for cases where a node may spin up but does not have any publishers or subscriptions. // Liveliness token for the node. zc_owned_liveliness_token_t token; + + // The entity id of this node as generated by get_next_entity_id(). + // Every interface created by this node will include this id in its liveliness token. + size_t id; }; ///============================================================================= diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index ebc97535..9c09f9db 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -286,9 +286,11 @@ rmw_create_node( // Initialize liveliness token for the node to advertise that a new node is in town. rmw_node_data_t * node_data = static_cast(node->data); + node_data->id = context->impl->get_next_entity_id(); const auto liveliness_entity = liveliness::Entity::make( z_info_zid(z_loan(context->impl->session)), - std::to_string(context->impl->get_next_entity_id()), + std::to_string(node_data->id), + std::to_string(node_data->id), liveliness::EntityType::Node, liveliness::NodeInfo{context->actual_domain_id, namespace_, name, ""}); if (!liveliness_entity.has_value()) { @@ -438,6 +440,13 @@ rmw_create_publisher( "Strict requirement on unique network flow endpoints for publishers not supported"); return nullptr; } + RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); + const rmw_node_data_t * node_data = static_cast(node->data); + if (node_data == nullptr) { + RMW_SET_ERROR_MSG( + "Unable to create publisher as node_data is invalid."); + return nullptr; + } // Get the RMW type support. const rosidl_message_type_support_t * type_support = find_message_type_support(type_supports); @@ -604,6 +613,7 @@ rmw_create_publisher( publisher_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), + std::to_string(node_data->id), std::to_string(context_impl->get_next_entity_id()), liveliness::EntityType::Publisher, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, @@ -633,6 +643,7 @@ rmw_create_publisher( "Unable to create liveliness token for the publisher."); return nullptr; } + printf("[rmw_create_publisher] Created pub %s\n", publisher_data->entity->keyexpr().c_str()); free_token.cancel(); undeclare_z_publisher_cache.cancel(); @@ -1147,6 +1158,7 @@ rmw_create_subscription( return nullptr; } + RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); auto node_data = static_cast(node->data); RMW_CHECK_FOR_NULL_WITH_MSG( node_data, "unable to create subscription as node_data is invalid.", @@ -1348,6 +1360,7 @@ rmw_create_subscription( // Publish to the graph that a new subscription is in town sub_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), + std::to_string(node_data->id), std::to_string(context_impl->get_next_entity_id()), liveliness::EntityType::Subscription, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, @@ -1832,6 +1845,13 @@ rmw_create_client( RMW_SET_ERROR_MSG("zenoh session is invalid"); return nullptr; } + RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); + const rmw_node_data_t * node_data = static_cast(node->data); + if (node_data == nullptr) { + RMW_SET_ERROR_MSG( + "Unable to create client as node data is invalid."); + return nullptr; + } rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -1991,6 +2011,7 @@ rmw_create_client( } client_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), + std::to_string(node_data->id), std::to_string(context_impl->get_next_entity_id()), liveliness::EntityType::Client, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, @@ -2467,7 +2488,13 @@ rmw_create_service( return nullptr; } } - + RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); + const rmw_node_data_t * node_data = static_cast(node->data); + if (node_data == nullptr) { + RMW_SET_ERROR_MSG( + "Unable to create service as node data is invalid."); + return nullptr; + } RMW_CHECK_FOR_NULL_WITH_MSG( node->context, "expected initialized context", @@ -2649,6 +2676,7 @@ rmw_create_service( } service_data->entity = liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), + std::to_string(node_data->id), std::to_string(context_impl->get_next_entity_id()), liveliness::EntityType::Service, liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, @@ -3165,12 +3193,12 @@ static bool has_triggered_condition( auto event_data = static_cast(event->data); if (event_data != nullptr) { if (!event_data->event_queue_is_empty(zenoh_event_it->second)) { - printf("EVENTS QUEUE IS NOT EMPTY!!\n"); return true; } } } else { - printf("ERROR!!!!!!!!!!!!!!\n"); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "has_triggered_condition() called with unknown event %u. Report this bug.", event_type); } } } From 5e3df35bee8f4024896342c43b89c63e7b376d23 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 26 Feb 2024 15:14:10 +0800 Subject: [PATCH 09/12] Debug race condition Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 36 ++++++++++++++++++++++-- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 4 +-- rmw_zenoh_cpp/src/rmw_init.cpp | 4 ++- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 10 +++---- 4 files changed, 43 insertions(+), 11 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index dec5ec38..71eb9394 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -204,6 +204,9 @@ void GraphCache::update_topic_map_for_put( static_cast(1)); if (is_entity_local(sub_entity)) { local_entities_with_events[sub_entity].insert(ZENOH_EVENT_SUBSCRIPTION_MATCHED); + printf( + "Updating matched count by 1 for local sub: %s\n", + sub_entity.keyexpr().c_str()); } } // Update event counters for the new entity. @@ -213,6 +216,9 @@ void GraphCache::update_topic_map_for_put( match_count_for_entity); if (is_entity_local(entity) && match_count_for_entity > 0) { local_entities_with_events[entity].insert(ZENOH_EVENT_PUBLICATION_MATCHED); + printf( + "Updating matched count by %ld for local pub: %s\n", + match_count_for_entity, entity.keyexpr().c_str()); } } else { // Entity is a sub. @@ -228,6 +234,9 @@ void GraphCache::update_topic_map_for_put( static_cast(1)); if (is_entity_local(pub_entity)) { local_entities_with_events[pub_entity].insert(ZENOH_EVENT_PUBLICATION_MATCHED); + printf( + "Updating matched count by 1 for local pub: %s\n", + pub_entity.keyexpr().c_str()); } } // Update event counters for the new entity. @@ -237,6 +246,9 @@ void GraphCache::update_topic_map_for_put( match_count_for_entity); if (is_entity_local(entity) && match_count_for_entity > 0) { local_entities_with_events[entity].insert(ZENOH_EVENT_SUBSCRIPTION_MATCHED); + printf( + "Updating matched count by %ld for local sub: %s\n", + match_count_for_entity, entity.keyexpr().c_str()); } } } @@ -294,7 +306,9 @@ void GraphCache::take_local_entities_with_events( } ///============================================================================= -void GraphCache::parse_put(const std::string & keyexpr) +void GraphCache::parse_put( + const std::string & keyexpr, + bool ignore_from_current_session) { printf("[parse_put %s] %s\n", zid_str_.c_str(), keyexpr.c_str()); std::optional maybe_entity = liveliness::Entity::make(keyexpr); @@ -304,6 +318,12 @@ void GraphCache::parse_put(const std::string & keyexpr) } const liveliness::Entity & entity = *maybe_entity; + if (ignore_from_current_session && is_entity_local(entity)){ + RCUTILS_LOG_DEBUG_NAMED( + "rmw_zenoh_cpp", + "Ignoring parse_put for %s from the same session.\n", entity.keyexpr().c_str()); + return; + } // Lock the graph mutex before accessing the graph. std::lock_guard lock(graph_mutex_); @@ -487,6 +507,9 @@ void GraphCache::update_topic_map_for_del( static_cast(-1)); if (is_entity_local(pub_entity)) { local_entities_with_events[pub_entity].insert(ZENOH_EVENT_PUBLICATION_MATCHED); + printf( + "Updating matched count by -1 for pub sub: %s\n", + pub_entity.keyexpr().c_str()); } } } @@ -544,7 +567,9 @@ void GraphCache::remove_topic_map_from_cache( } ///============================================================================= -void GraphCache::parse_del(const std::string & keyexpr) +void GraphCache::parse_del( +const std::string & keyexpr, +bool ignore_from_current_session) { printf("[parse_del %s] %s\n", zid_str_.c_str(), keyexpr.c_str()); std::optional maybe_entity = liveliness::Entity::make(keyexpr); @@ -553,7 +578,12 @@ void GraphCache::parse_del(const std::string & keyexpr) return; } const liveliness::Entity entity = *maybe_entity; - + if (ignore_from_current_session && is_entity_local(entity)){ + RCUTILS_LOG_DEBUG_NAMED( + "rmw_zenoh_cpp", + "Ignoring parse_del for %s from the same session.\n", entity.keyexpr().c_str()); + return; + } // Lock the graph mutex before accessing the graph. std::lock_guard lock(graph_mutex_); diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 2ebd363c..e07140be 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -101,9 +101,9 @@ class GraphCache final explicit GraphCache(const z_id_t & zid); // Parse a PUT message over a token's key-expression and update the graph. - void parse_put(const std::string & keyexpr); + void parse_put(const std::string & keyexpr, bool ignore_from_current_session = false); // Parse a DELETE message over a token's key-expression and update the graph. - void parse_del(const std::string & keyexpr); + void parse_del(const std::string & keyexpr, bool ignore_from_current_session = false); rmw_ret_t get_node_names( rcutils_string_array_t * node_names, diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 399b6b60..90e0bf1c 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -282,7 +282,9 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) if (z_reply_is_ok(&reply)) { z_sample_t sample = z_reply_ok(&reply); z_owned_str_t keystr = z_keyexpr_to_string(sample.keyexpr); - context->impl->graph_cache->parse_put(z_loan(keystr)); + // Ignore tokens from the same session to avoid race conditions from this + // query and the liveliness subscription. + context->impl->graph_cache->parse_put(z_loan(keystr), true); z_drop(z_move(keystr)); } else { printf("[discovery] Received an error\n"); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 9c09f9db..051f3774 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -341,7 +341,7 @@ rmw_destroy_node(rmw_node_t * node) // Undeclare liveliness token for the node to advertise that the node has ridden // off into the sunset. rmw_node_data_t * node_data = static_cast(node->data); - z_drop(z_move(node_data->token)); + zc_liveliness_undeclare_token(z_move(node_data->token)); rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -683,7 +683,7 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) auto publisher_data = static_cast(publisher->data); if (publisher_data != nullptr) { - z_drop(z_move(publisher_data->token)); + zc_liveliness_undeclare_token(z_move(publisher_data->token)); if (publisher_data->pub_cache.has_value()) { z_drop(z_move(publisher_data->pub_cache.value())); } @@ -1428,7 +1428,7 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) auto sub_data = static_cast(subscription->data); if (sub_data != nullptr) { // Publish to the graph that a subscription has ridden off into the sunset - z_drop(z_move(sub_data->token)); + zc_liveliness_undeclare_token(z_move(sub_data->token)); RMW_TRY_DESTRUCTOR(sub_data->type_support->~MessageTypeSupport(), MessageTypeSupport, ); allocator->deallocate(sub_data->type_support, allocator->state); @@ -2088,7 +2088,7 @@ rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) // CLEANUP =================================================================== z_drop(z_move(client_data->zn_closure_reply)); z_drop(z_move(client_data->keyexpr)); - z_drop(z_move(client_data->token)); + zc_liveliness_undeclare_token(z_move(client_data->token)); RMW_TRY_DESTRUCTOR( client_data->request_type_support->~RequestTypeSupport(), RequestTypeSupport, ); @@ -2755,7 +2755,7 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) // CLEANUP ================================================================ z_drop(z_move(service_data->keyexpr)); z_undeclare_queryable(z_move(service_data->qable)); - z_drop(z_move(service_data->token)); + zc_liveliness_undeclare_token(z_move(service_data->token)); RMW_TRY_DESTRUCTOR( service_data->request_type_support->~RequestTypeSupport(), RequestTypeSupport, ); From 07c27f32905a0e195fd4d4b6720fd677f92291c0 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 26 Feb 2024 16:11:43 +0800 Subject: [PATCH 10/12] Update qos compatibility check to reflect zenoh's behavior Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 12 ++--- rmw_zenoh_cpp/src/rmw_qos.cpp | 66 +++++++++++++++++++++--- 2 files changed, 64 insertions(+), 14 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 71eb9394..5cfe76dd 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -318,7 +318,7 @@ void GraphCache::parse_put( } const liveliness::Entity & entity = *maybe_entity; - if (ignore_from_current_session && is_entity_local(entity)){ + if (ignore_from_current_session && is_entity_local(entity)) { RCUTILS_LOG_DEBUG_NAMED( "rmw_zenoh_cpp", "Ignoring parse_put for %s from the same session.\n", entity.keyexpr().c_str()); @@ -568,8 +568,8 @@ void GraphCache::remove_topic_map_from_cache( ///============================================================================= void GraphCache::parse_del( -const std::string & keyexpr, -bool ignore_from_current_session) + const std::string & keyexpr, + bool ignore_from_current_session) { printf("[parse_del %s] %s\n", zid_str_.c_str(), keyexpr.c_str()); std::optional maybe_entity = liveliness::Entity::make(keyexpr); @@ -578,7 +578,7 @@ bool ignore_from_current_session) return; } const liveliness::Entity entity = *maybe_entity; - if (ignore_from_current_session && is_entity_local(entity)){ + if (ignore_from_current_session && is_entity_local(entity)) { RCUTILS_LOG_DEBUG_NAMED( "rmw_zenoh_cpp", "Ignoring parse_del for %s from the same session.\n", entity.keyexpr().c_str()); @@ -876,7 +876,7 @@ rmw_ret_t GraphCache::publisher_count_matched_subscriptions( &is_compatible, nullptr, 0); - if (ret == RMW_RET_OK && is_compatible == RMW_QOS_COMPATIBILITY_OK) { + if (ret == RMW_RET_OK && is_compatible != RMW_QOS_COMPATIBILITY_ERROR) { *subscription_count = *subscription_count + topic_data->stats_.subs_.size(); } } @@ -911,7 +911,7 @@ rmw_ret_t GraphCache::subscription_count_matched_publishers( &is_compatible, nullptr, 0); - if (ret == RMW_RET_OK && is_compatible == RMW_QOS_COMPATIBILITY_OK) { + if (ret == RMW_RET_OK && is_compatible != RMW_QOS_COMPATIBILITY_ERROR) { *publisher_count = *publisher_count + topic_data->stats_.pubs_.size(); } } diff --git a/rmw_zenoh_cpp/src/rmw_qos.cpp b/rmw_zenoh_cpp/src/rmw_qos.cpp index 0dcf6a4d..dd566727 100644 --- a/rmw_zenoh_cpp/src/rmw_qos.cpp +++ b/rmw_zenoh_cpp/src/rmw_qos.cpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + +#include "rcutils/snprintf.h" + #include "rmw/error_handling.h" #include "rmw/types.h" #include "rmw/qos_profiles.h" @@ -20,6 +24,30 @@ extern "C" { +// Copied from rmw_dds_common::qos.cpp. +// Returns RMW_RET_OK if successful or no buffer was provided +// Returns RMW_RET_ERROR if there as an error copying the message to the buffer +static rmw_ret_t +_append_to_buffer(char * buffer, size_t buffer_size, const char * format, ...) +{ + // Only write if a buffer is provided + if (!buffer || buffer_size == 0u) { + return RMW_RET_OK; + } + // Determine available space left in buffer + size_t offset = strnlen(buffer, buffer_size); + size_t write_size = buffer_size - offset; + std::va_list args; + va_start(args, format); + int snprintf_ret = rcutils_vsnprintf(buffer + offset, write_size, format, args); + va_end(args); + if (snprintf_ret < 0) { + RMW_SET_ERROR_MSG("failed to append to character buffer"); + return RMW_RET_ERROR; + } + return RMW_RET_OK; +} + rmw_ret_t rmw_qos_profile_check_compatible( const rmw_qos_profile_t publisher_profile, @@ -28,13 +56,35 @@ rmw_qos_profile_check_compatible( char * reason, size_t reason_size) { - // In Zenoh, publishers do not have any reliability settings. - // A publisher and subscription are only incompatible if the durability of the publisher is - // TRANSIENT_LOCAL but that of the subscription is not. In such a scenario, a late-joining - // subscription can fail to receive messages so we flag it accordingly. - // However, we can reuse the qos_profile_check_compatible() method from rmw_dds_common - // since it largely applies in rmw_zenoh. - return rmw_dds_common::qos_profile_check_compatible( - publisher_profile, subscription_profile, compatibility, reason, reason_size); + if (!compatibility) { + RMW_SET_ERROR_MSG("compatibility parameter is null"); + return RMW_RET_INVALID_ARGUMENT; + } + if (!reason && reason_size != 0u) { + RMW_SET_ERROR_MSG("reason parameter is null, but reason_size parameter is not zero"); + return RMW_RET_INVALID_ARGUMENT; + } + // Initialize reason buffer + if (reason && reason_size != 0u) { + reason[0] = '\0'; + } + // In Zenoh, there are not qos incompatibilities. + // Further publishers do not have any reliability settings. + // The once scenario where transport may not occur is where a publisher with + // TRANSIENT_LOCAL durability publishes a message before a subscription with + // VOLATILE durability spins up. However, any subsequent messages published + // will be received by the subscription. + if (publisher_profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL && + subscription_profile.durability == RMW_QOS_POLICY_DURABILITY_VOLATILE) + { + *compatibility = RMW_QOS_COMPATIBILITY_WARNING; + return _append_to_buffer( + reason, + reason_size, + "WARNING: Publisher's durability is TRANSIENT_LOCAL, but subscription's is VOLATILE;"); + } else { + *compatibility = RMW_QOS_COMPATIBILITY_OK; + } + return RMW_RET_OK; } } // extern "C" From a5b5acfb3db593996c333efad381ea78d4e35fde Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 26 Feb 2024 18:31:14 +0800 Subject: [PATCH 11/12] Get rid of TopicStats Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 67 +++++++++++++----------- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 21 +++----- 2 files changed, 43 insertions(+), 45 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 5cfe76dd..6623f1d1 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -43,9 +43,11 @@ using EntityType = liveliness::EntityType; ///============================================================================= TopicData::TopicData( liveliness::TopicInfo info, - TopicStats stats) + std::unordered_set pubs, + std::unordered_set subs) : info_(std::move(info)), - stats_(std::move(stats)) + pubs_(std::move(pubs)), + subs_(std::move(subs)) {} ///============================================================================= @@ -123,16 +125,17 @@ void GraphCache::update_topic_map_for_put( // Initialize a map that will be populated with any QoS events that may be detected. std::unordered_map> local_entities_with_events = {}; - - TopicStats new_topic_stats; + std::unordered_set pubs = {}; + std::unordered_set subs = {}; if (is_pub) { - new_topic_stats.pubs_.insert(entity); + pubs.insert(entity); } else { - new_topic_stats.subs_.insert(entity); + subs.insert(entity); } TopicDataPtr graph_topic_data = std::make_shared( topic_info, - std::move(new_topic_stats)); + std::move(pubs), + std::move(subs)); std::string qos_str = liveliness::qos_to_keyexpr(topic_info.qos_); GraphNode::TopicQoSMap topic_qos_map = { @@ -193,11 +196,11 @@ void GraphCache::update_topic_map_for_put( for (const auto & [_, topic_data_ptr] : topic_type_map_it->second) { if (is_pub) { // Count the number of matching subs for each set of qos settings. - if (!topic_data_ptr->stats_.subs_.empty()) { - match_count_for_entity += topic_data_ptr->stats_.subs_.size(); + if (!topic_data_ptr->subs_.empty()) { + match_count_for_entity += topic_data_ptr->subs_.size(); } // Also iterate through the subs to check if any are local and if update event counters. - for (const liveliness::Entity & sub_entity : topic_data_ptr->stats_.subs_) { + for (const liveliness::Entity & sub_entity : topic_data_ptr->subs_) { update_event_counters( topic_info.name_, ZENOH_EVENT_SUBSCRIPTION_MATCHED, @@ -223,11 +226,11 @@ void GraphCache::update_topic_map_for_put( } else { // Entity is a sub. // Count the number of matching pubs for each set of qos settings. - if (!topic_data_ptr->stats_.pubs_.empty()) { - match_count_for_entity += topic_data_ptr->stats_.pubs_.size(); + if (!topic_data_ptr->pubs_.empty()) { + match_count_for_entity += topic_data_ptr->pubs_.size(); } // Also iterate through the pubs to check if any are local and if update event counters. - for (const liveliness::Entity & pub_entity : topic_data_ptr->stats_.pubs_) { + for (const liveliness::Entity & pub_entity : topic_data_ptr->pubs_) { update_event_counters( topic_info.name_, ZENOH_EVENT_PUBLICATION_MATCHED, @@ -265,9 +268,9 @@ void GraphCache::update_topic_map_for_put( // type and qos so we increment the counters. TopicDataPtr & existing_graph_topic = topic_qos_map_it->second; if (is_pub) { - existing_graph_topic->stats_.pubs_.insert(entity); + existing_graph_topic->pubs_.insert(entity); } else { - existing_graph_topic->stats_.subs_.insert(entity); + existing_graph_topic->subs_.insert(entity); } } } @@ -464,14 +467,14 @@ void GraphCache::update_topic_map_for_del( } // Decrement the relevant counters. If both counters are 0 remove from cache. if (is_pub) { - cache_topic_qos_it->second->stats_.pubs_.erase(entity); + cache_topic_qos_it->second->pubs_.erase(entity); } else { - cache_topic_qos_it->second->stats_.subs_.erase(entity); + cache_topic_qos_it->second->subs_.erase(entity); } // If after removing the entity, the parent map is empty, then remove parent // map. - if (cache_topic_qos_it->second->stats_.pubs_.empty() && - cache_topic_qos_it->second->stats_.subs_.empty()) + if (cache_topic_qos_it->second->pubs_.empty() && + cache_topic_qos_it->second->subs_.empty()) { cache_topic_type_it->second.erase(qos_str); } @@ -484,7 +487,7 @@ void GraphCache::update_topic_map_for_del( if (is_pub) { // Notify any local subs of a matched event with change -1. for (const auto & [_, topic_data_ptr] : cache_topic_type_it->second) { - for (const liveliness::Entity & sub_entity : topic_data_ptr->stats_.subs_) { + for (const liveliness::Entity & sub_entity : topic_data_ptr->subs_) { update_event_counters( topic_info.name_, ZENOH_EVENT_SUBSCRIPTION_MATCHED, @@ -500,7 +503,7 @@ void GraphCache::update_topic_map_for_del( } else { // Notify any local pubs of a matched event with change -1. for (const auto & [_, topic_data_ptr] : cache_topic_type_it->second) { - for (const liveliness::Entity & pub_entity : topic_data_ptr->stats_.pubs_) { + for (const liveliness::Entity & pub_entity : topic_data_ptr->pubs_) { update_event_counters( topic_info.name_, ZENOH_EVENT_PUBLICATION_MATCHED, @@ -549,13 +552,13 @@ void GraphCache::remove_topic_map_from_cache( // Technically only one of pubs_ or sub_ will be populated and with one // element at most since to_remove comes from a node. For completeness, // we iterate though both and call update_topic_map_for_del(). - for (const liveliness::Entity & entity : topic_qos_it->second->stats_.pubs_) { + for (const liveliness::Entity & entity : topic_qos_it->second->pubs_) { update_topic_map_for_del( from_cache, entity, true); } - for (const liveliness::Entity & entity : topic_qos_it->second->stats_.subs_) { + for (const liveliness::Entity & entity : topic_qos_it->second->subs_) { update_topic_map_for_del( from_cache, entity, @@ -868,7 +871,7 @@ rmw_ret_t GraphCache::publisher_count_matched_subscriptions( if (topic_data_it != topic_it->second.end()) { for (const auto & [_, topic_data] : topic_data_it->second) { // If a subscription exists with compatible QoS, update the subscription count. - if (!topic_data->stats_.subs_.empty()) { + if (!topic_data->subs_.empty()) { rmw_qos_compatibility_type_t is_compatible; rmw_ret_t ret = rmw_qos_profile_check_compatible( pub_data->adapted_qos_profile, @@ -877,7 +880,7 @@ rmw_ret_t GraphCache::publisher_count_matched_subscriptions( nullptr, 0); if (ret == RMW_RET_OK && is_compatible != RMW_QOS_COMPATIBILITY_ERROR) { - *subscription_count = *subscription_count + topic_data->stats_.subs_.size(); + *subscription_count = *subscription_count + topic_data->subs_.size(); } } } @@ -903,7 +906,7 @@ rmw_ret_t GraphCache::subscription_count_matched_publishers( if (topic_data_it != topic_it->second.end()) { for (const auto & [_, topic_data] : topic_data_it->second) { // If a subscription exists with compatible QoS, update the subscription count. - if (!topic_data->stats_.pubs_.empty()) { + if (!topic_data->pubs_.empty()) { rmw_qos_compatibility_type_t is_compatible; rmw_ret_t ret = rmw_qos_profile_check_compatible( sub_data->adapted_qos_profile, @@ -912,7 +915,7 @@ rmw_ret_t GraphCache::subscription_count_matched_publishers( nullptr, 0); if (ret == RMW_RET_OK && is_compatible != RMW_QOS_COMPATIBILITY_ERROR) { - *publisher_count = *publisher_count + topic_data->stats_.pubs_.size(); + *publisher_count = *publisher_count + topic_data->pubs_.size(); } } } @@ -947,7 +950,7 @@ rmw_ret_t GraphCache::count_publishers( GraphNode::TopicQoSMap> & topic_data : graph_topics_.at(topic_name)) { for (auto it = topic_data.second.begin(); it != topic_data.second.end(); ++it) { - *count += it->second->stats_.pubs_.size(); + *count += it->second->pubs_.size(); } } } @@ -968,7 +971,7 @@ rmw_ret_t GraphCache::count_subscriptions( GraphNode::TopicQoSMap> & topic_data : graph_topics_.at(topic_name)) { for (auto it = topic_data.second.begin(); it != topic_data.second.end(); ++it) { - *count += it->second->stats_.subs_.size(); + *count += it->second->subs_.size(); } } } @@ -989,7 +992,7 @@ rmw_ret_t GraphCache::count_services( GraphNode::TopicQoSMap> & topic_data : graph_services_.at(service_name)) { for (auto it = topic_data.second.begin(); it != topic_data.second.end(); ++it) { - *count += it->second->stats_.subs_.size(); + *count += it->second->subs_.size(); } } } @@ -1010,7 +1013,7 @@ rmw_ret_t GraphCache::count_clients( GraphNode::TopicQoSMap> & topic_data : graph_services_.at(service_name)) { for (auto it = topic_data.second.begin(); it != topic_data.second.end(); ++it) { - *count += it->second->stats_.pubs_.size(); + *count += it->second->pubs_.size(); } } } @@ -1219,7 +1222,7 @@ rmw_ret_t GraphCache::service_server_is_available( GraphNode::TopicTypeMap::iterator type_it = service_it->second.find(service_type); if (type_it != service_it->second.end()) { for (const auto & [_, topic_data] : type_it->second) { - if (topic_data->stats_.subs_.size() > 0) { + if (topic_data->subs_.size() > 0) { *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 index e07140be..d00be06f 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -38,28 +38,23 @@ ///============================================================================= // TODO(Yadunund): Since we reuse pub_count_ and sub_count_ for pub/sub and // service/client consider more general names for these fields. -// Consider changing this to an array of unordered_maps where the index of the +// Consider changing this to an array of unordered_set where the index of the // array corresponds to the EntityType enum. This way we don't need to mix // pub/sub with client/service. -struct TopicStats +struct TopicData { - // The guids of publishers or clients. + liveliness::TopicInfo info_; + + // The publishers or clients entities. std::unordered_set pubs_; - // The guids of subscriptions or services. + // The subscriptions or services entities std::unordered_set subs_; -}; - -///============================================================================= -// TODO(Yadunund): Simplify to directly store pubs_ and subs_. -struct TopicData -{ - liveliness::TopicInfo info_; - TopicStats stats_; TopicData( liveliness::TopicInfo info, - TopicStats stats); + std::unordered_set pubs, + std::unordered_set subs); }; using TopicDataPtr = std::shared_ptr; From 29f3a79f039ef679fdf9a75e6cd233d6445ae7aa Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 26 Feb 2024 22:39:22 +0800 Subject: [PATCH 12/12] Replace EventsBase inheritance with DataCallbackManager and EventsManager objects Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/event.cpp | 24 +++++++++---------- rmw_zenoh_cpp/src/detail/event.hpp | 25 ++++++++++++++++---- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 26 +-------------------- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 6 ++--- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 17 ++++++++++---- rmw_zenoh_cpp/src/rmw_event.cpp | 19 +++++---------- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 15 ++++++------ 7 files changed, 63 insertions(+), 69 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/event.cpp b/rmw_zenoh_cpp/src/detail/event.cpp index 4b99e419..2b204c91 100644 --- a/rmw_zenoh_cpp/src/detail/event.cpp +++ b/rmw_zenoh_cpp/src/detail/event.cpp @@ -22,10 +22,10 @@ ///============================================================================= -void EventsBase::set_user_callback( +void DataCallbackManager::set_callback( const void * user_data, rmw_event_callback_t callback) { - std::lock_guard lock_mutex(event_mutex_); + std::lock_guard lock_mutex(event_mutex_); if (callback) { // Push events arrived before setting the the executor callback. @@ -42,10 +42,10 @@ void EventsBase::set_user_callback( } ///============================================================================= -void EventsBase::trigger_user_callback() +void DataCallbackManager::trigger_callback() { // Trigger the user provided event callback if available. - std::lock_guard lock_mutex(event_mutex_); + std::lock_guard lock_mutex(event_mutex_); if (callback_ != nullptr) { callback_(user_data_, 1); } else { @@ -54,7 +54,7 @@ void EventsBase::trigger_user_callback() } ///============================================================================= -void EventsBase::event_set_callback( +void EventsManager::event_set_callback( rmw_zenoh_event_type_t event_id, rmw_event_callback_t callback, const void * user_data) @@ -82,7 +82,7 @@ void EventsBase::event_set_callback( } ///============================================================================= -void EventsBase::trigger_event_callback(rmw_zenoh_event_type_t event_id) +void EventsManager::trigger_event_callback(rmw_zenoh_event_type_t event_id) { if (event_id > ZENOH_EVENT_ID_MAX) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -103,7 +103,7 @@ void EventsBase::trigger_event_callback(rmw_zenoh_event_type_t event_id) } ///============================================================================= -bool EventsBase::event_queue_is_empty(rmw_zenoh_event_type_t event_id) const +bool EventsManager::event_queue_is_empty(rmw_zenoh_event_type_t event_id) const { if (event_id > ZENOH_EVENT_ID_MAX) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -119,7 +119,7 @@ bool EventsBase::event_queue_is_empty(rmw_zenoh_event_type_t event_id) const } ///============================================================================= -std::unique_ptr EventsBase::pop_next_event( +std::unique_ptr EventsManager::pop_next_event( rmw_zenoh_event_type_t event_id) { if (event_id > ZENOH_EVENT_ID_MAX) { @@ -145,7 +145,7 @@ std::unique_ptr EventsBase::pop_next_event( } ///============================================================================= -void EventsBase::add_new_event( +void EventsManager::add_new_event( rmw_zenoh_event_type_t event_id, std::unique_ptr event) { @@ -180,7 +180,7 @@ void EventsBase::add_new_event( } ///============================================================================= -void EventsBase::attach_event_condition( +void EventsManager::attach_event_condition( rmw_zenoh_event_type_t event_id, std::condition_variable * condition_variable) { @@ -197,7 +197,7 @@ void EventsBase::attach_event_condition( } ///============================================================================= -void EventsBase::detach_event_condition(rmw_zenoh_event_type_t event_id) +void EventsManager::detach_event_condition(rmw_zenoh_event_type_t event_id) { if (event_id > ZENOH_EVENT_ID_MAX) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -212,7 +212,7 @@ void EventsBase::detach_event_condition(rmw_zenoh_event_type_t event_id) } ///============================================================================= -void EventsBase::notify_event(rmw_zenoh_event_type_t event_id) +void EventsManager::notify_event(rmw_zenoh_event_type_t event_id) { if (event_id > ZENOH_EVENT_ID_MAX) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( diff --git a/rmw_zenoh_cpp/src/detail/event.hpp b/rmw_zenoh_cpp/src/detail/event.hpp index 0adc05cb..01dbce4c 100644 --- a/rmw_zenoh_cpp/src/detail/event.hpp +++ b/rmw_zenoh_cpp/src/detail/event.hpp @@ -82,19 +82,34 @@ struct rmw_zenoh_event_status_t }; ///============================================================================= -/// Base class to be inherited by entities that support events. -class EventsBase +/// A class that manages callbacks that should be triggered when a new +/// message/request/response is received by an entity. +class DataCallbackManager { public: /// @brief Set the user defined callback that should be called when /// a new message/response/request is received. /// @param user_data the data that should be passed to the callback. /// @param callback the callback to be set. - void set_user_callback(const void * user_data, rmw_event_callback_t callback); + void set_callback(const void * user_data, rmw_event_callback_t callback); /// Trigger the user callback. - void trigger_user_callback(); + void trigger_callback(); +private: + std::mutex event_mutex_; + /// User callback that can be set via set_callback(). + rmw_event_callback_t callback_ {nullptr}; + /// User data that should be passed to the user callback. + const void * user_data_ {nullptr}; + /// number of trigger requests made before the callback was set. + size_t unread_count_ {0}; +}; + +/// Base class to be inherited by entities that support events. +class EventsManager +{ +public: /// @brief Set the callback to be triggered when the relevant event is triggered. /// @param event_id the id of the event /// @param callback the callback to trigger for this event. @@ -143,7 +158,7 @@ class EventsBase mutable std::mutex event_condition_mutex_; /// Condition variable to attach for event notifications. std::condition_variable * event_conditions_[ZENOH_EVENT_ID_MAX + 1]{nullptr}; - /// User callback that can be set via set_user_callback(). + /// User callback that can be set via data_callback_mgr.set_callback(). rmw_event_callback_t callback_ {nullptr}; /// User data that should be passed to the user callback. const void * user_data_ {nullptr}; diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 6623f1d1..1a820dbb 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -154,31 +154,7 @@ void GraphCache::update_topic_map_for_put( topic_info.type_); if (topic_type_map_it == topic_map_it->second.end()) { // First time this topic type is added. - - // Check for and report an *_INCOMPATIBLE_TYPE event if a different type for the same - // topic exists. -// if (report_events) { -// for (const auto & [topic_type, qos_map] : topic_map_it->second) { -// for (const auto & [qos_type, topic_data_ptr] : qos_map) { -// // If the entity is a publisher but the graph contains a sub, -// // report ZENOH_EVENT_PUBLISHER_INCOMPATIBLE_TYPE or -// // ZENOH_EVENT_PUBLISHER_INCOMPATIBLE_TYPE if vice versa. -// if ((is_pub && topic_data_ptr->stats_.sub_count_ > 0) || -// (!is_pub && topic_data_ptr->stats_.pub_count_ > 0)) -// { -// goto incompatible_type_event_found; -// } -// } -// } -// incompatible_type_event_found: -// // TODO(Yadunund): Retrieve total count from global counters. -// auto event_type = is_pub ? ZENOH_EVENT_PUBLISHER_INCOMPATIBLE_TYPE : -// ZENOH_EVENT_SUBSCRIPTION_INCOMPATIBLE_TYPE; -// auto event = std::make_unique(); -// event->total_count = 1; -// event->total_count_change = 1; -// detected_events[event_type] = std::move(event); -// } + // TODO(Yadunund) Check for and report an *_INCOMPATIBLE_TYPE events. topic_map_it->second.insert( std::make_pair( diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 2037c80b..5a374aaa 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -121,7 +121,7 @@ void rmw_subscription_data_t::add_new_message( message_queue_.emplace_back(std::move(msg)); // Since we added new data, trigger user callback and guard condition if they are available - trigger_user_callback(); + data_callback_mgr.trigger_callback(); notify(); } @@ -176,7 +176,7 @@ void rmw_service_data_t::add_new_query(std::unique_ptr query) query_queue_.emplace_back(std::move(query)); // Since we added new data, trigger user callback and guard condition if they are available - trigger_user_callback(); + data_callback_mgr.trigger_callback(); notify(); } @@ -225,7 +225,7 @@ void rmw_client_data_t::add_new_reply(std::unique_ptr reply) reply_queue_.emplace_back(std::move(reply)); // Since we added new data, trigger user callback and guard condition if they are available - trigger_user_callback(); + data_callback_mgr.trigger_callback(); notify(); } diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 01a9b9ce..6f7490aa 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -83,7 +83,7 @@ struct rmw_node_data_t }; ///============================================================================= -class rmw_publisher_data_t : public EventsBase +class rmw_publisher_data_t { public: // The Entity generated for the publisher. @@ -108,6 +108,8 @@ class rmw_publisher_data_t : public EventsBase // Context for memory allocation for messages. rmw_context_t * context; + + EventsManager events_mgr; }; ///============================================================================= @@ -135,7 +137,7 @@ struct saved_msg_data }; ///============================================================================= -class rmw_subscription_data_t : public EventsBase +class rmw_subscription_data_t { public: // The Entity generated for the subscription. @@ -165,6 +167,9 @@ class rmw_subscription_data_t : public EventsBase void add_new_message(std::unique_ptr msg, const std::string & topic_name); + DataCallbackManager data_callback_mgr; + EventsManager events_mgr; + private: std::deque> message_queue_; mutable std::mutex message_queue_mutex_; @@ -197,7 +202,7 @@ class ZenohQuery final }; ///============================================================================= -class rmw_service_data_t : public EventsBase +class rmw_service_data_t { public: // The Entity generated for the service. @@ -235,6 +240,8 @@ class rmw_service_data_t : public EventsBase std::unique_ptr take_from_query_map(int64_t sequence_number); + DataCallbackManager data_callback_mgr; + private: void notify(); @@ -265,7 +272,7 @@ class ZenohReply final }; ///============================================================================= -class rmw_client_data_t : public EventsBase +class rmw_client_data_t { public: // The Entity generated for the client. @@ -303,6 +310,8 @@ class rmw_client_data_t : public EventsBase std::unique_ptr pop_next_reply(); + DataCallbackManager data_callback_mgr; + private: void notify(); diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index 99654ec8..a22cdcf8 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -54,7 +54,7 @@ rmw_publisher_event_init( } rmw_event->implementation_identifier = publisher->implementation_identifier; - rmw_event->data = pub_data; + rmw_event->data = &pub_data->events_mgr; rmw_event->event_type = event_type; // Register the event with graph cache. @@ -67,15 +67,12 @@ rmw_publisher_event_init( if (pub_data == nullptr) { return; } - pub_data->add_new_event( + pub_data->events_mgr.add_new_event( event_id, std::move(zenoh_event)); } ); - // printf( - // "[rmw_publisher_event_init] created new rmw_event_type_t %s for %s\n", - // std::to_string(event_type).c_str(), pub_data->entity->keyexpr().c_str()); return RMW_RET_OK; } @@ -109,7 +106,7 @@ rmw_subscription_event_init( } rmw_event->implementation_identifier = subscription->implementation_identifier; - rmw_event->data = sub_data; + rmw_event->data = &sub_data->events_mgr; rmw_event->event_type = event_type; // Register the event with graph cache if the event is not ZENOH_EVENT_MESSAGE_LOST @@ -127,15 +124,12 @@ rmw_subscription_event_init( if (sub_data == nullptr) { return; } - sub_data->add_new_event( + sub_data->events_mgr.add_new_event( event_id, std::move(zenoh_event)); } ); - // printf( - // "[rmw_subscription_event_init] created new rmw_event_type_t %s for %s\n", - // std::to_string(event_type).c_str(), sub_data->entity->keyexpr().c_str()); return RMW_RET_OK; } @@ -159,7 +153,7 @@ rmw_event_set_callback( } // Both rmw_subscription_data_t and rmw_publisher_data_t inherit EventsBase. - EventsBase * event_data = static_cast(rmw_event->data); + EventsManager * event_data = static_cast(rmw_event->data); RMW_CHECK_ARGUMENT_FOR_NULL(event_data, RMW_RET_INVALID_ARGUMENT); event_data->event_set_callback( zenoh_event_it->second, @@ -196,8 +190,7 @@ rmw_take_event( return RMW_RET_ERROR; } - // Both rmw_subscription_data_t and rmw_publisher_data_t inherit EventsBase. - EventsBase * event_data = static_cast(event_handle->data); + EventsManager * event_data = static_cast(event_handle->data); RMW_CHECK_ARGUMENT_FOR_NULL(event_data, RMW_RET_INVALID_ARGUMENT); std::unique_ptr st = event_data->pop_next_event( zenoh_event_it->second); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 051f3774..f67df6d5 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1261,7 +1261,6 @@ rmw_create_subscription( sub_data->context = node->context; rmw_subscription->implementation_identifier = rmw_zenoh_identifier; - rmw_subscription->data = sub_data; rmw_subscription->topic_name = rcutils_strdup(topic_name, *allocator); RMW_CHECK_FOR_NULL_WITH_MSG( @@ -1391,6 +1390,8 @@ rmw_create_subscription( return nullptr; } + rmw_subscription->data = sub_data; + free_token.cancel(); undeclare_z_sub.cancel(); free_topic_name.cancel(); @@ -3190,7 +3191,7 @@ static bool has_triggered_condition( // Check if the event queue for this event type is empty. auto zenoh_event_it = event_map.find(event_type); if (zenoh_event_it != event_map.end()) { - auto event_data = static_cast(event->data); + auto event_data = static_cast(event->data); if (event_data != nullptr) { if (!event_data->event_queue_is_empty(zenoh_event_it->second)) { return true; @@ -3334,7 +3335,7 @@ rmw_wait( if (events) { for (size_t i = 0; i < events->event_count; ++i) { auto event = static_cast(events->events[i]); - auto event_data = static_cast(event->data); + auto event_data = static_cast(event->data); if (event_data != nullptr) { auto zenoh_event_it = event_map.find(event->event_type); if (zenoh_event_it != event_map.end()) { @@ -3381,7 +3382,7 @@ rmw_wait( // Now detach the condition variable and mutex from each of the subscriptions for (size_t i = 0; i < events->event_count; ++i) { auto event = static_cast(events->events[i]); - auto event_data = static_cast(event->data); + auto event_data = static_cast(event->data); if (event_data != nullptr) { auto zenoh_event_it = event_map.find(event->event_type); if (zenoh_event_it != event_map.end()) { @@ -3719,7 +3720,7 @@ rmw_subscription_set_on_new_message_callback( rmw_subscription_data_t * sub_data = static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); - sub_data->set_user_callback( + sub_data->data_callback_mgr.set_callback( user_data, callback); return RMW_RET_OK; } @@ -3736,7 +3737,7 @@ rmw_service_set_on_new_request_callback( rmw_service_data_t * service_data = static_cast(service->data); RMW_CHECK_ARGUMENT_FOR_NULL(service_data, RMW_RET_INVALID_ARGUMENT); - service_data->set_user_callback( + service_data->data_callback_mgr.set_callback( user_data, callback); return RMW_RET_OK; } @@ -3753,7 +3754,7 @@ rmw_client_set_on_new_response_callback( rmw_client_data_t * client_data = static_cast(client->data); RMW_CHECK_ARGUMENT_FOR_NULL(client_data, RMW_RET_INVALID_ARGUMENT); - client_data->set_user_callback( + client_data->data_callback_mgr.set_callback( user_data, callback); return RMW_RET_OK; }