diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index c17920913..f40f8b7ba 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -442,7 +442,7 @@ Entity::Entity( sizeof(keyexpr_gid.high64)); // We also hash the liveliness keyexpression into a size_t that we use to index into our maps. - this->keyexpr_hash_ = hash_gid(this->gid_); + this->keyexpr_hash_ = std::hash{}(this->gid_); } ///============================================================================= @@ -670,15 +670,4 @@ std::string demangle_name(const std::string & input) return output; } } // namespace liveliness - -///============================================================================= -size_t hash_gid(const std::array gid) -{ - std::stringstream hash_str; - hash_str << std::hex; - for (const auto & g : gid) { - hash_str << static_cast(g); - } - return std::hash{}(hash_str.str()); -} } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp index 8577619fd..a14b9acce 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp @@ -234,8 +234,7 @@ std::string qos_to_keyexpr(const rmw_qos_profile_t & qos); std::optional keyexpr_to_qos(const std::string & keyexpr); } // namespace liveliness -///============================================================================= -size_t hash_gid(const std::array gid); +using Gid = std::array; } // namespace rmw_zenoh_cpp ///============================================================================= @@ -270,6 +269,29 @@ struct equal_to return lhs->keyexpr_hash() == rhs->keyexpr_hash(); } }; + +template<> +struct hash +{ + std::size_t operator()(const rmw_zenoh_cpp::Gid & gid) const noexcept + { + // This function implemented FNV-1a 64-bit as the GID is small enough + // (e.g. as of commit db4d05e of ros2/rmw RMW_GID_STORAGE_SIZE is set to 16) + // and FNV is known to be efficient in such cases. + // + // See https://github.com/ros2/rmw/blob/db4d05e/rmw/include/rmw/types.h#L44 + // and https://datatracker.ietf.org/doc/html/draft-eastlake-fnv-17.html + static constexpr uint64_t FNV_OFFSET_BASIS_64 = 0x00000100000001b3; + static constexpr uint64_t FNV_PRIME_64 = 0xcbf29ce484222325; + + uint64_t hash = FNV_OFFSET_BASIS_64; + for (const uint8_t & byte : gid) { + hash ^= byte; + hash *= FNV_PRIME_64; + } + return static_cast(hash); + } +}; } // namespace std #endif // DETAIL__LIVELINESS_UTILS_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp index 289aeb7d3..8087349c0 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp @@ -346,12 +346,11 @@ rmw_ret_t ServiceData::take_request( request_header->received_timestamp = query->get_received_timestamp(); // Add this query to the map, so that rmw_send_response can quickly look it up later. - const size_t hash = rmw_zenoh_cpp::hash_gid(writer_guid); - std::unordered_map::iterator it = sequence_to_query_map_.find(hash); + std::unordered_map::iterator it = sequence_to_query_map_.find(writer_guid); if (it == sequence_to_query_map_.end()) { SequenceToQuery stq; - sequence_to_query_map_.insert(std::make_pair(hash, std::move(stq))); - it = sequence_to_query_map_.find(hash); + sequence_to_query_map_.insert(std::make_pair(writer_guid, std::move(stq))); + it = sequence_to_query_map_.find(writer_guid); } else { // Client already in the map if (it->second.find(request_header->request_id.sequence_number) != it->second.end()) { @@ -380,12 +379,11 @@ rmw_ret_t ServiceData::send_response( return RMW_RET_OK; } - std::array writer_guid; + Gid writer_guid; memcpy(writer_guid.data(), request_id->writer_guid, RMW_GID_STORAGE_SIZE); // Create the queryable payload - const size_t hash = hash_gid(writer_guid); - std::unordered_map::iterator it = sequence_to_query_map_.find(hash); + std::unordered_map::iterator it = sequence_to_query_map_.find(writer_guid); if (it == sequence_to_query_map_.end()) { // If there is no data associated with this request, the higher layers of // ROS 2 seem to expect that we just silently return with no work. @@ -399,8 +397,8 @@ rmw_ret_t ServiceData::send_response( } std::unique_ptr query = std::move(query_it->second); it->second.erase(query_it); - if (sequence_to_query_map_[hash].size() == 0) { - sequence_to_query_map_.erase(hash); + if (sequence_to_query_map_[writer_guid].size() == 0) { + sequence_to_query_map_.erase(writer_guid); } rcutils_allocator_t * allocator = &(rmw_node_->context->options.allocator); diff --git a/rmw_zenoh_cpp/src/detail/rmw_service_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_service_data.hpp index 1c557370b..d556b54bd 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_service_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_service_data.hpp @@ -149,7 +149,7 @@ class ServiceData final : public std::enable_shared_from_this std::deque> query_queue_; // Map to store the sequence_number (as given by the client) -> ZenohQuery using SequenceToQuery = std::unordered_map>; - std::unordered_map sequence_to_query_map_; + std::unordered_map sequence_to_query_map_; // Wait set data. rmw_wait_set_data_t * wait_set_data_; // Data callback manager. diff --git a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp index cb7a54ab7..35fdd7b4e 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp @@ -610,8 +610,8 @@ void SubscriptionData::add_new_message( } // Check for messages lost if the new sequence number is not monotonically increasing. - const size_t gid_hash = hash_gid(msg->attachment.copy_gid()); - auto last_known_pub_it = last_known_published_msg_.find(gid_hash); + const Gid gid = msg->attachment.copy_gid(); + auto last_known_pub_it = last_known_published_msg_.find(gid); if (last_known_pub_it != last_known_published_msg_.end()) { const int64_t seq_increment = std::abs( msg->attachment.sequence_number() - @@ -624,7 +624,7 @@ void SubscriptionData::add_new_message( } } // Always update the last known sequence number for the publisher. - last_known_published_msg_[gid_hash] = msg->attachment.sequence_number(); + last_known_published_msg_[gid] = msg->attachment.sequence_number(); message_queue_.emplace_back(std::move(msg)); diff --git a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp index 81c8c1071..74491be7e 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp @@ -147,7 +147,7 @@ class SubscriptionData final : public std::enable_shared_from_this type_support_; std::deque> message_queue_; // Map GID of a subscription to the sequence number of the message it published. - std::unordered_map last_known_published_msg_; + std::unordered_map last_known_published_msg_; // Wait set data. rmw_wait_set_data_t * wait_set_data_; // Callback managers.