From 1017890b1c44d97bb3eeb9ade57eb9ff2f1415fb Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:14:58 +0000 Subject: [PATCH 1/9] Refs #20164: rmw_fastrtps_shared: prepare api for keys support Signed-off-by: Mario Dominguez --- .../include/rmw_fastrtps_shared_cpp/TypeSupport.hpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp index 7da7a0d640..214aae4a2e 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp @@ -61,15 +61,14 @@ class TypeSupport : public eprosima::fastdds::dds::TopicDataType virtual bool deserializeROSmessage( eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const = 0; + virtual bool getKeyHashFromROSmessage( + void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, const void * impl) const = 0; + RMW_FASTRTPS_SHARED_CPP_PUBLIC bool getKey( void * data, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, - bool force_md5 = false) override - { - (void)data; (void)ihandle; (void)force_md5; - return false; - } + bool force_md5 = false) override; RMW_FASTRTPS_SHARED_CPP_PUBLIC bool serialize(void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) override; From 8f1faee0d5cdb14f46e911bf99c79d564253092d Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:15:45 +0000 Subject: [PATCH 2/9] Refs #20164: rmw_fastrtps_shared: getKey() implementation Signed-off-by: Mario Dominguez --- .../src/TypeSupport_impl.cpp | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp index f357dd28eb..05766ea56c 100644 --- a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -64,6 +64,50 @@ void * TypeSupport::createData() return new eprosima::fastcdr::FastBuffer(); } +bool TypeSupport::getKey( + void * data, + eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, + bool force_md5) +{ + assert(data); + + bool ret = false; + + if (!m_isGetKeyDefined) + { + return ret; + } + + auto ser_data = static_cast(data); + + switch (ser_data->type) + { + case FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE: + { + ret = this->getKeyHashFromROSmessage(ser_data->data, ihandle, force_md5, ser_data->impl); + break; + } + + case FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER: + { + //! TODO + break; + } + + case FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE: + { + //! TODO + break; + } + default: + { + break; + } + } + + return ret; +} + bool TypeSupport::serialize( void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) { From 3b7de8af421eda9de409a0d5bbb4894586471c64 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:20:02 +0000 Subject: [PATCH 3/9] Refs #20164: rmw_fastrtps* interface methods update for keys support Signed-off-by: Mario Dominguez --- .../include/rmw_fastrtps_cpp/TypeSupport.hpp | 4 +++ .../rmw_fastrtps_dynamic_cpp/TypeSupport.hpp | 30 +++++++++++++++++-- 2 files changed, 32 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp index 6323ca35a0..762d888017 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp @@ -42,6 +42,9 @@ 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: @@ -49,6 +52,7 @@ class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport private: const message_type_support_callbacks_t * members_; + message_type_support_key_callbacks_t key_callbacks_; bool has_data_; }; diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp index 270839275e..915388cb9f 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp @@ -23,6 +23,7 @@ #include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" +#include "fastrtps/utils/md5.h" #include "rcutils/logging_macros.h" @@ -138,6 +139,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 +175,27 @@ 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_; + bool key_is_unbounded_; + mutable size_t key_max_serialized_size_; + mutable MD5 md5_; + mutable std::vector key_buffer_; + 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 +206,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 From 9c358cab95d5edb62873607c60000f15acb4e8ae Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:20:47 +0000 Subject: [PATCH 4/9] Refs #20164: rmw_fastrtps_cpp empty implementation Signed-off-by: Mario Dominguez --- rmw_fastrtps_cpp/src/type_support_common.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/rmw_fastrtps_cpp/src/type_support_common.cpp b/rmw_fastrtps_cpp/src/type_support_common.cpp index 4e5dff1dc1..2d42d8c959 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_cpp/src/type_support_common.cpp @@ -33,6 +33,7 @@ TypeSupport::TypeSupport() void TypeSupport::set_members(const message_type_support_callbacks_t * members) { members_ = members; + m_isGetKeyDefined = members->get_key_type_support(&key_callbacks_); #ifdef ROSIDL_TYPESUPPORT_FASTRTPS_HAS_PLAIN_TYPES char bounds_info; @@ -124,6 +125,16 @@ bool TypeSupport::deserializeROSmessage( return true; } +bool TypeSupport::getKeyHashFromROSmessage( + void *, + eprosima::fastrtps::rtps::InstanceHandle_t *, + bool, + const void *) const +{ + //!TODO + return false; +} + MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * members) { assert(members); From 03adfb35eed805db0801e7a0bdfb90ae0829051b Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:23:47 +0000 Subject: [PATCH 5/9] Refs #20164: rmw_fastrtps_dynamic_cpp Message & Service Typesupport construction updates Signed-off-by: Mario Dominguez --- .../MessageTypeSupport_impl.hpp | 9 ++++++++- .../ServiceTypeSupport_impl.hpp | 18 ++++++++++++++++-- 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp index 95f48e996f..6e9e11c3f0 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp @@ -56,10 +56,17 @@ MessageTypeSupport::MessageTypeSupport( // Encapsulation size this->m_typeSize = 4; if (this->members_->member_count_ != 0) { - this->m_typeSize += static_cast(this->calculateMaxSerializedSize(members, 0)); + this->m_typeSize += static_cast(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; } diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp index 45e3fc6028..2eaf321558 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp @@ -55,10 +55,17 @@ RequestTypeSupport::RequestTypeSupport( // Encapsulation size this->m_typeSize = 4; if (this->members_->member_count_ != 0) { - this->m_typeSize += static_cast(this->calculateMaxSerializedSize(this->members_, 0)); + this->m_typeSize += static_cast(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::ResponseTypeSupport // Encapsulation size this->m_typeSize = 4; if (this->members_->member_count_ != 0) { - this->m_typeSize += static_cast(this->calculateMaxSerializedSize(this->members_, 0)); + this->m_typeSize += static_cast(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; } From 45f48f1effcbe25f2807284882e9ae8afffb6222 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:24:50 +0000 Subject: [PATCH 6/9] Refs #20164: rmw_fastrtps_dynamic_cpp type_support_proxy updates implementation Signed-off-by: Mario Dominguez --- rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp index 43c3b0ac42..14a478d127 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp @@ -23,6 +23,7 @@ TypeSupportProxy::TypeSupportProxy(rmw_fastrtps_shared_cpp::TypeSupport * inner_ m_typeSize = inner_type->m_typeSize; is_plain_ = inner_type->is_plain(); max_size_bound_ = inner_type->is_bounded(); + m_isGetKeyDefined = inner_type->m_isGetKeyDefined; } size_t TypeSupportProxy::getEstimatedSerializedSize( @@ -46,4 +47,11 @@ bool TypeSupportProxy::deserializeROSmessage( return type_impl->deserializeROSmessage(deser, ros_message, impl); } +bool TypeSupportProxy::getKeyHashFromROSmessage( + void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, const void * impl) const +{ + auto type_impl = static_cast(impl); + return type_impl->getKeyHashFromROSmessage(ros_message, ihandle, force_md5, impl); +} + } // namespace rmw_fastrtps_dynamic_cpp From f4f25c43b9902f9e7cb48e686296479ce9d77b75 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:25:34 +0000 Subject: [PATCH 7/9] Refs #20164: rmw_fastrtps_dynamic_cpp type_support implementation Signed-off-by: Mario Dominguez --- .../TypeSupport_impl.hpp | 245 +++++++++++++++++- 1 file changed, 240 insertions(+), 5 deletions(-) diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp index f673fe0735..ae428ffe98 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp @@ -67,6 +67,8 @@ TypeSupport::TypeSupport(const void * ros_type_support) m_isGetKeyDefined = false; max_size_bound_ = false; is_plain_ = false; + key_max_serialized_size_ = 0; + key_is_unbounded_ = false; } // C++ specialization @@ -299,6 +301,165 @@ bool TypeSupport::serializeROSmessage( return true; } +template +bool TypeSupport::serializeKeyROSmessage( + eprosima::fastcdr::Cdr & ser, + const MembersType * members, + void * ros_message, + bool check_if_member_is_key) const +{ + for (uint32_t i = 0; i < members->member_count_; ++i) { + const auto member = members->members_ + i; + + if (check_if_member_is_key && !member->is_key_) + { + continue; + } + + void * field = const_cast(static_cast(ros_message)) + member->offset_; + switch (member->type_id_) { + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: + if (!member->is_array_) { + // don't cast to bool here because if the bool is + // uninitialized the random value can't be deserialized + ser << (*static_cast(field) ? true : false); + } else { + serialize_field(member, field, ser); + } + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + { + auto sub_members = static_cast(member->members_->data); + if (!member->is_array_) { + serializeKeyROSmessage(ser, sub_members, field, false); + } else { + size_t array_size = 0; + + if (member->array_size_ && !member->is_upper_bound_) { + array_size = member->array_size_; + } else { + if (!member->size_function) { + RMW_SET_ERROR_MSG("unexpected error: size function is null"); + return false; + } + array_size = member->size_function(field); + + // Serialize length + ser << (uint32_t)array_size; + } + + if (array_size != 0 && !member->get_function) { + RMW_SET_ERROR_MSG("unexpected error: get_function function is null"); + return false; + } + for (size_t index = 0; index < array_size; ++index) { + serializeKeyROSmessage(ser, sub_members, member->get_function(field, index), false); + } + } + } + break; + default: + throw std::runtime_error("unknown type"); + } + } + + return true; +} + +template +bool TypeSupport::getKeyHashFromROSmessage( + const MembersType * members, + void * ros_message, + eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, + bool force_md5) const +{ + assert(members); + assert(ros_message); + + // get estimated serialized size in case key is unbounded + if (this->key_is_unbounded_) + { + this->key_max_serialized_size_ = this->getEstimatedSerializedSize(members, ros_message, 0, true); + key_buffer_.reserve(this->key_max_serialized_size_); + } + + eprosima::fastcdr::FastBuffer buffer( + reinterpret_cast(this->key_buffer_.data()), + this->key_max_serialized_size_); + + eprosima::fastcdr::Cdr ser( + buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + + // serialize + serializeKeyROSmessage(ser, members_, ros_message, true); + + // check for md5 + if (force_md5 || this->key_max_serialized_size_ > 16) + { + md5_.init(); + +#if FASTCDR_VERSION_MAJOR == 1 + md5_.update(this->key_buffer_.data(), static_cast(ser.getSerializedDataLength())); +#else + md5_.update(this->key_buffer_.data(), static_cast(ser.get_serialized_data_length())); +#endif // FASTCDR_VERSION_MAJOR == 1 + + 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]; + } + } + + return true; +} + // C++ specialization template size_t next_field_align( @@ -465,7 +626,8 @@ template size_t TypeSupport::getEstimatedSerializedSize( const MembersType * members, const void * ros_message, - size_t current_alignment) const + size_t current_alignment, + bool compute_key_members_only) const { assert(members); assert(ros_message); @@ -475,6 +637,12 @@ size_t TypeSupport::getEstimatedSerializedSize( for (uint32_t i = 0; i < members->member_count_; ++i) { const auto member = members->members_ + i; void * field = const_cast(static_cast(ros_message)) + member->offset_; + + if (compute_key_members_only && !member->is_key_) + { + continue; + } + switch (member->type_id_) { case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: current_alignment = next_field_align(member, field, current_alignment); @@ -521,7 +689,7 @@ size_t TypeSupport::getEstimatedSerializedSize( { auto sub_members = static_cast(member->members_->data); if (!member->is_array_) { - current_alignment += getEstimatedSerializedSize(sub_members, field, current_alignment); + current_alignment += getEstimatedSerializedSize(sub_members, field, current_alignment, compute_key_members_only); } else { size_t array_size = 0; @@ -546,7 +714,8 @@ size_t TypeSupport::getEstimatedSerializedSize( current_alignment += getEstimatedSerializedSize( sub_members, member->get_function(field, index), - current_alignment); + current_alignment, + compute_key_members_only); } } } @@ -816,7 +985,7 @@ bool TypeSupport::deserializeROSmessage( template size_t TypeSupport::calculateMaxSerializedSize( - const MembersType * members, size_t current_alignment) + const MembersType * members, size_t current_alignment, size_t& max_serialized_key_size) { assert(members); @@ -829,6 +998,13 @@ size_t TypeSupport::calculateMaxSerializedSize( const auto * member = members->members_ + i; size_t array_size = 1; + bool member_is_key = false; + + if (member->is_key_) + { + member_is_key = true; + } + if (member->is_array_) { array_size = member->array_size_; @@ -842,6 +1018,12 @@ size_t TypeSupport::calculateMaxSerializedSize( this->is_plain_ = false; current_alignment += padding + eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + + if (member_is_key) + { + max_serialized_key_size += padding + + eprosima::fastcdr::Cdr::alignment(max_serialized_key_size, padding); + } } } @@ -854,12 +1036,22 @@ size_t TypeSupport::calculateMaxSerializedSize( case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: last_member_size = array_size * sizeof(int8_t); current_alignment += array_size * sizeof(int8_t); + if (member_is_key) + { + max_serialized_key_size += array_size * sizeof(uint8_t) + + eprosima::fastcdr::Cdr::alignment(max_serialized_key_size, sizeof(uint8_t)); + } break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: last_member_size = array_size * sizeof(uint16_t); current_alignment += array_size * sizeof(uint16_t) + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint16_t)); + if (member_is_key) + { + max_serialized_key_size += array_size * sizeof(uint16_t) + + eprosima::fastcdr::Cdr::alignment(max_serialized_key_size, sizeof(uint16_t)); + } break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: @@ -867,6 +1059,11 @@ size_t TypeSupport::calculateMaxSerializedSize( last_member_size = array_size * sizeof(uint32_t); current_alignment += array_size * sizeof(uint32_t) + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t)); + if (member_is_key) + { + max_serialized_key_size += array_size * sizeof(uint32_t) + + eprosima::fastcdr::Cdr::alignment(max_serialized_key_size, sizeof(uint32_t)); + } break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: @@ -874,6 +1071,11 @@ size_t TypeSupport::calculateMaxSerializedSize( last_member_size = array_size * sizeof(uint64_t); current_alignment += array_size * sizeof(uint64_t) + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t)); + if (member_is_key) + { + max_serialized_key_size += array_size * sizeof(uint64_t) + + eprosima::fastcdr::Cdr::alignment(max_serialized_key_size, sizeof(uint64_t)); + } break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: @@ -886,6 +1088,13 @@ size_t TypeSupport::calculateMaxSerializedSize( current_alignment += padding + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + character_size * (member->string_upper_bound_ + 1); + if (member_is_key) + { + key_is_unbounded_ = true; + max_serialized_key_size += padding + + eprosima::fastcdr::Cdr::alignment(max_serialized_key_size, padding) + + character_size * (member->string_upper_bound_ + 1); + } } } break; @@ -893,9 +1102,16 @@ size_t TypeSupport::calculateMaxSerializedSize( { auto sub_members = static_cast(member->members_->data); for (size_t index = 0; index < array_size; ++index) { - size_t curr = calculateMaxSerializedSize(sub_members, current_alignment); + // We set the type as keyed + // if any of the parent struct members are keyed + size_t dummy_max_serialized_key_size; + size_t curr = calculateMaxSerializedSize(sub_members, current_alignment, dummy_max_serialized_key_size); current_alignment += curr; last_member_size += curr; + if (member_is_key) + { + max_serialized_key_size += curr; + } } } break; @@ -988,6 +1204,25 @@ bool TypeSupport::deserializeROSmessage( return true; } +template +bool TypeSupport::getKeyHashFromROSmessage( + void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, const void * impl) const +{ + + assert(ros_message); + assert(members_); + + bool ret = false; + + (void)impl; + if (members_->member_count_ != 0) + { + ret = TypeSupport::getKeyHashFromROSmessage(members_, ros_message, ihandle, force_md5); + } + + return ret; +} + } // namespace rmw_fastrtps_dynamic_cpp #endif // RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ From 641fb853ea5a26c40b7064f944c3fb9a4dc41a3a Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Tue, 30 Jan 2024 11:32:50 +0000 Subject: [PATCH 8/9] Refs #20164: Move typesupport key vars to rmw_fastrtps_shared_cpp Signed-off-by: Mario Dominguez --- .../include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp | 6 ------ .../include/rmw_fastrtps_shared_cpp/TypeSupport.hpp | 5 +++++ 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp index 915388cb9f..84142f41c2 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp @@ -23,7 +23,6 @@ #include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" -#include "fastrtps/utils/md5.h" #include "rcutils/logging_macros.h" @@ -185,11 +184,6 @@ class TypeSupport : public BaseTypeSupport const MembersType * members_; - bool key_is_unbounded_; - mutable size_t key_max_serialized_size_; - mutable MD5 md5_; - mutable std::vector key_buffer_; - private: size_t getEstimatedSerializedSize( const MembersType * members, diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp index 214aae4a2e..6812842bfa 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp @@ -25,6 +25,7 @@ #include "fastcdr/FastBuffer.h" #include "fastcdr/Cdr.h" +#include "fastrtps/utils/md5.h" #include "rcutils/logging_macros.h" @@ -112,6 +113,10 @@ class TypeSupport : public eprosima::fastdds::dds::TopicDataType bool max_size_bound_; bool is_plain_; + bool key_is_unbounded_; + mutable size_t key_max_serialized_size_; + mutable MD5 md5_; + mutable std::vector key_buffer_; }; RMW_FASTRTPS_SHARED_CPP_PUBLIC From f46729cc8fef6f656b5dc03e2bbb018c3fd22deb Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Tue, 30 Jan 2024 11:34:21 +0000 Subject: [PATCH 9/9] Refs #20164: Initialize Typesupport key members in rmw_fastrtps_cpp Signed-off-by: Mario Dominguez --- rmw_fastrtps_cpp/src/type_support_common.cpp | 88 ++++++++++++++++++-- 1 file changed, 81 insertions(+), 7 deletions(-) diff --git a/rmw_fastrtps_cpp/src/type_support_common.cpp b/rmw_fastrtps_cpp/src/type_support_common.cpp index 2d42d8c959..79d5e58122 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_cpp/src/type_support_common.cpp @@ -28,12 +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(&key_callbacks_); + m_isGetKeyDefined = members->get_key_type_support(&this->key_callbacks_); #ifdef ROSIDL_TYPESUPPORT_FASTRTPS_HAS_PLAIN_TYPES char bounds_info; @@ -54,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 @@ -126,13 +145,68 @@ bool TypeSupport::deserializeROSmessage( } bool TypeSupport::getKeyHashFromROSmessage( - void *, - eprosima::fastrtps::rtps::InstanceHandle_t *, - bool, - const void *) const + void * ros_message, + eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, + bool force_md5, + const void * impl) const { - //!TODO - return false; + 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(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(ser.getSerializedDataLength())); +#else + this->md5_.update(this->key_buffer_.data(), static_cast(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)