Using unordered_set

Signed-off-by: Miguel Company <[email protected]>

rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp

@@ -19,7 +19,7 @@
 #include <condition_variable>
 #include <list>
 #include <mutex>
-#include <set>
+#include <unordered_set>
 
 #include "fastcdr/FastBuffer.h"
 
@@ -33,6 +33,7 @@
 #include "rcpputils/thread_safety_annotations.hpp"
 
 #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp"
+#include "rmw_fastrtps_shared_cpp/guid_utils.hpp"
 
 class ServiceListener;
 class ServicePubListener;
@@ -204,8 +205,12 @@ class ServicePubListener : public eprosima::fastrtps::PublisherListener
   }
 
 private:
+  using subscriptions_set_t =
+    std::unordered_set<eprosima::fastrtps::rtps::GUID_t,
+      rmw_fastrtps_shared_cpp::hash_fastrtps_guid>;
+
   std::mutex mutex_;
-  std::set<eprosima::fastrtps::rtps::GUID_t> subscriptions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  subscriptions_set_t subscriptions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
   std::condition_variable cv_;
 };
 

rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp

@@ -16,6 +16,7 @@
 #define RMW_FASTRTPS_SHARED_CPP__GUID_UTILS_HPP_
 
 #include <cassert>
+#include <cstddef>
 #include <cstring>
 #include <type_traits>
 
@@ -55,6 +56,36 @@ copy_from_fastrtps_guid_to_byte_array(
   memcpy(&guid_byte_array[prefix_size], &guid.entityId, guid.entityId.size);
 }
 
+struct hash_fastrtps_guid
+{
+  std::size_t operator()(const eprosima::fastrtps::rtps::GUID_t & guid) const
+  {
+    union u_convert
+    {
+      uint8_t plain_value[sizeof(guid)];
+      uint32_t plain_ints[4];
+    } u;
+
+    static_assert(
+      sizeof(u.plain_value) == sizeof(u.plain_ints) &&
+      offsetof(u_convert, plain_value) == offsetof(u_convert, plain_ints),
+      "ByteT should be either int8_t or uint8_t");
+
+    copy_from_fastrtps_guid_to_byte_array(guid, u.plain_value);
+
+    constexpr std::size_t prime_1 = 7;
+    constexpr std::size_t prime_2 = 31;
+    constexpr std::size_t prime_3 = 59;
+
+    size_t ret_val = prime_1 * u.plain_ints[0];
+    ret_val = prime_2 * (u.plain_ints[1] + ret_val);
+    ret_val = prime_3 * (u.plain_ints[2] + ret_val);
+    ret_val = u.plain_ints[3] + ret_val;
+
+    return ret_val;
+  }
+};
+
 }  // namespace rmw_fastrtps_shared_cpp
 
 #endif  // RMW_FASTRTPS_SHARED_CPP__GUID_UTILS_HPP_