[20164] Feature: topic keys

rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp

@@ -42,13 +42,17 @@ class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport
   bool deserializeROSmessage(
     eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override;
 
+  bool getKeyHashFromROSmessage(
+    void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, const void * impl) const override;
+
   TypeSupport();
 
 protected:
   void set_members(const message_type_support_callbacks_t * members);
 
 private:
   const message_type_support_callbacks_t * members_;
+  message_type_support_key_callbacks_t  key_callbacks_;
   bool has_data_;
 };
 

rmw_fastrtps_cpp/src/type_support_common.cpp

@@ -28,11 +28,14 @@ TypeSupport::TypeSupport()
   m_isGetKeyDefined = false;
   max_size_bound_ = false;
   is_plain_ = false;
+  key_is_unbounded_ = false;
+  key_max_serialized_size_ = 0;
 }
 
 void TypeSupport::set_members(const message_type_support_callbacks_t * members)
 {
   members_ = members;
+  m_isGetKeyDefined = members->get_key_type_support(&this->key_callbacks_);
 
 #ifdef ROSIDL_TYPESUPPORT_FASTRTPS_HAS_PLAIN_TYPES
   char bounds_info;
@@ -53,6 +56,23 @@ void TypeSupport::set_members(const message_type_support_callbacks_t * members)
     has_data_ = true;
   }
 
+  if (m_isGetKeyDefined)
+  {
+    std::cout << "Calculating max_serialized_key_size " << (&key_callbacks_.max_serialized_key_size) << std::endl;
+    this->key_max_serialized_size_ = this->key_callbacks_.max_serialized_key_size(0, this->key_is_unbounded_);
+    std::cout << "FInishing max_serialized_key_size" << std::endl;
+    if (!this->key_is_unbounded_)
+    {
+      this->key_buffer_.reserve(this->key_max_serialized_size_);
+    }
+    else
+    {
+      std::cout << "KEY is UNBOUNDED" << std::endl;
+    }
+
+    std::cout << "this->key_max_serialized_size_ " << this->key_max_serialized_size_ << std::endl;
+  }
+
   // Total size is encapsulation size + data size
   m_typeSize = 4 + data_size;
   // Account for RTPS submessage alignment
@@ -124,6 +144,71 @@ bool TypeSupport::deserializeROSmessage(
   return true;
 }
 
+bool TypeSupport::getKeyHashFromROSmessage(
+    void * ros_message,
+    eprosima::fastrtps::rtps::InstanceHandle_t * ihandle,
+    bool force_md5,
+    const void * impl) const
+{
+  assert(ros_message);
+  (void)impl;
+
+  /*if (capacity < 16)
+  {
+    throw std::runtime_error("Not enough capacity to serialize key");
+  }*/
+
+  //! retrieve estimated serialized size in case key is unbounded
+  if (this->key_is_unbounded_)
+  {
+    std::cout << "Static Re-stimating serialize size. Before " << this->key_max_serialized_size_ << std::endl;
+    this->key_max_serialized_size_ = key_callbacks_.get_serialized_key_size(ros_message, 0);
+    std::cout << "Static Re-stimating serialize size. After " << this->key_max_serialized_size_ << std::endl;
+    this->key_buffer_.reserve(this->key_max_serialized_size_);
+    std::cout << "Static New key buffer capacity " << this->key_buffer_.capacity() << std::endl;
+  }
+
+  eprosima::fastcdr::FastBuffer fast_buffer(
+    reinterpret_cast<char *>(this->key_buffer_.data()),
+    this->key_max_serialized_size_);
+
+  eprosima::fastcdr::Cdr ser(
+    fast_buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+
+  key_callbacks_.cdr_serialize_key(ros_message, ser);
+
+    //! check for md5
+  if (force_md5 || this->key_max_serialized_size_ > 16)
+  {
+
+    this->md5_.init();
+
+#if FASTCDR_VERSION_MAJOR == 1
+    this->md5_.update(this->key_buffer_.data(), static_cast<unsigned int>(ser.getSerializedDataLength()));
+#else
+    this->md5_.update(this->key_buffer_.data(), static_cast<unsigned int>(ser.get_serialized_data_length()));
+#endif // FASTCDR_VERSION_MAJOR == 1
+
+    this->md5_.finalize();
+
+    for (uint8_t i = 0; i < 16; ++i)
+    {
+      ihandle->value[i] = md5_.digest[i];
+    }
+  }
+  else
+  {
+    for (uint8_t i = 0; i < 16; ++i)
+    {
+        ihandle->value[i] = this->key_buffer_[i];
+    }
+  }
+
+  std::cout << "\nFinishing static::getKeyHashFromROSmessage() "  << ihandle->value << std::endl;
+
+  return true;
+}
+
 MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * members)
 {
   assert(members);

rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp

@@ -56,10 +56,17 @@ MessageTypeSupport<MembersType>::MessageTypeSupport(
   // Encapsulation size
   this->m_typeSize = 4;
   if (this->members_->member_count_ != 0) {
-    this->m_typeSize += static_cast<uint32_t>(this->calculateMaxSerializedSize(members, 0));
+    this->m_typeSize += static_cast<uint32_t>(this->calculateMaxSerializedSize(members, 0, this->key_max_serialized_size_));
   } else {
     this->m_typeSize++;
   }
+
+  if (this->key_max_serialized_size_ != 0)
+  {
+    this->m_isGetKeyDefined = true;
+    this->key_buffer_.reserve(this->key_max_serialized_size_);
+  }
+
   // Account for RTPS submessage alignment
   this->m_typeSize = (this->m_typeSize + 3) & ~3;
 }

rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp

@@ -55,10 +55,17 @@ RequestTypeSupport<ServiceMembersType, MessageMembersType>::RequestTypeSupport(
   // Encapsulation size
   this->m_typeSize = 4;
   if (this->members_->member_count_ != 0) {
-    this->m_typeSize += static_cast<uint32_t>(this->calculateMaxSerializedSize(this->members_, 0));
+    this->m_typeSize += static_cast<uint32_t>(this->calculateMaxSerializedSize(this->members_, 0, this->key_max_serialized_size_));
   } else {
     this->m_typeSize++;
   }
+
+  if (this->key_max_serialized_size_ != 0)
+  {
+    this->m_isGetKeyDefined = true;
+    this->key_buffer_.reserve(this->key_max_serialized_size_);
+  }
+
   // Account for RTPS submessage alignment
   this->m_typeSize = (this->m_typeSize + 3) & ~3;
 }
@@ -88,10 +95,17 @@ ResponseTypeSupport<ServiceMembersType, MessageMembersType>::ResponseTypeSupport
   // Encapsulation size
   this->m_typeSize = 4;
   if (this->members_->member_count_ != 0) {
-    this->m_typeSize += static_cast<uint32_t>(this->calculateMaxSerializedSize(this->members_, 0));
+    this->m_typeSize += static_cast<uint32_t>(this->calculateMaxSerializedSize(this->members_, 0, this->key_max_serialized_size_));
   } else {
     this->m_typeSize++;
   }
+
+  if (this->key_max_serialized_size_ != 0)
+  {
+    this->m_isGetKeyDefined = true;
+    this->key_buffer_.reserve(this->key_max_serialized_size_);
+  }
+
   // Account for RTPS submessage alignment
   this->m_typeSize = (this->m_typeSize + 3) & ~3;
 }

rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp

@@ -138,6 +138,10 @@ class TypeSupportProxy : public rmw_fastrtps_shared_cpp::TypeSupport
 
   bool deserializeROSmessage(
     eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override;
+
+  bool getKeyHashFromROSmessage(
+    void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, const void * impl) const override;
+
 };
 
 class BaseTypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport
@@ -170,18 +174,22 @@ class TypeSupport : public BaseTypeSupport
   bool deserializeROSmessage(
     eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override;
 
+  bool getKeyHashFromROSmessage(
+    void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, const void * impl) const override;
+
 protected:
   explicit TypeSupport(const void * ros_type_support);
 
-  size_t calculateMaxSerializedSize(const MembersType * members, size_t current_alignment);
+  size_t calculateMaxSerializedSize(const MembersType * members, size_t current_alignment, size_t& max_key_size);
 
   const MembersType * members_;
 
 private:
   size_t getEstimatedSerializedSize(
     const MembersType * members,
     const void * ros_message,
-    size_t current_alignment) const;
+    size_t current_alignment,
+    bool compute_key_members_only = false) const;
 
   bool serializeROSmessage(
     eprosima::fastcdr::Cdr & ser,
@@ -192,6 +200,18 @@ class TypeSupport : public BaseTypeSupport
     eprosima::fastcdr::Cdr & deser,
     const MembersType * members,
     void * ros_message) const;
+
+  bool serializeKeyROSmessage(
+    eprosima::fastcdr::Cdr & ser,
+    const MembersType * members,
+    void * ros_message,
+    bool check_if_member_is_key) const;
+
+  bool getKeyHashFromROSmessage(
+    const MembersType * members,
+    void * ros_message,
+    eprosima::fastrtps::rtps::InstanceHandle_t * ihandle,
+    bool force_md5) const;
 };
 
 }  // namespace rmw_fastrtps_dynamic_cpp