From 7e65f3815e43c6f7693256434b264b7e086dfe85 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Luis=20Bueno=20L=C3=B3pez?= <69244257+JLBuenoLopez-eProsima@users.noreply.github.com> Date: Wed, 19 Jan 2022 10:34:44 +0100 Subject: [PATCH 01/20] Add support for PKCS#11 in security files (#5) * Add support for PKCS#11 in security files In addition to *.pem files containing the key or certificate, adds support for *.p11 files that contain the PKCS#11 URI of the key or certificate in an HSM. Signed-off-by: Iker Luengo * linters Signed-off-by: Iker Luengo Co-authored-by: Iker Luengo --- rmw_fastrtps_shared_cpp/src/participant.cpp | 119 +++++++++++++++++++- 1 file changed, 118 insertions(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 6b6c9fe32d..9841dfb04b 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -17,6 +17,9 @@ #include #include #include +#include +#include +#include #include "fastdds/dds/core/status/StatusMask.hpp" #include "fastdds/dds/domain/DomainParticipantFactory.hpp" @@ -35,6 +38,7 @@ #include "fastrtps/utils/IPLocator.h" #include "rcpputils/scope_exit.hpp" +#include "rcpputils/filesystem_helper.hpp" #include "rcutils/env.h" #include "rcutils/filesystem.h" @@ -142,6 +146,119 @@ __create_participant( return participant_info; } +// Processor for security attributes with FILE URI +bool process_file_uri_security_file( + const std::string & prefix, const rcpputils::fs::path & full_path, + std::string & result) +{ + if (!full_path.is_regular_file()) { + return false; + } + result = prefix + full_path.string(); + return true; +} + +// Processor for security attributes with PKCS#11 URI +bool process_pkcs_uri_security_file( + const std::string & /*prefix*/, const rcpputils::fs::path & full_path, + std::string & result) +{ + const std::string p11_prefix("pkcs11:"); + + std::ifstream ifs(full_path.string()); + if (!ifs.is_open()) { + return false; + } + + if (!(ifs >> result)) { + return false; + } + if (result.find(p11_prefix) != 0) { + return false; + } + + return true; +} + +bool get_security_files( + const std::string & prefix, const std::string & secure_root, + std::unordered_map & result) +{ + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + using security_file_processor = + std::function; + using processor_vector = + std::vector>; + + // Key: the security attribute + // Value: ordered sequence of pairs. Each pair contains one possible file name + // for the attribute and the corresponding processor method + // Pairs are ordered by priority: the first one matching is used. + const std::unordered_map required_files{ + {"IDENTITY_CA", { + {"identity_ca.cert.pem", std::bind(process_file_uri_security_file, _1, _2, _3)}, + {"identity_ca.cert.p11", std::bind(process_pkcs_uri_security_file, _1, _2, _3)}}}, + {"CERTIFICATE", { + {"cert.pem", std::bind(process_file_uri_security_file, _1, _2, _3)}, + {"cert.p11", std::bind(process_pkcs_uri_security_file, _1, _2, _3)}}}, + {"PRIVATE_KEY", { + {"key.pem", std::bind(process_file_uri_security_file, _1, _2, _3)}, + {"key.p11", std::bind(process_pkcs_uri_security_file, _1, _2, _3)}}}, + {"PERMISSIONS_CA", { + {"permissions_ca.cert.pem", std::bind(process_file_uri_security_file, _1, _2, _3)}, + {"permissions_ca.cert.p11", std::bind(process_pkcs_uri_security_file, _1, _2, _3)}}}, + {"GOVERNANCE", { + {"governance.p7s", std::bind(process_file_uri_security_file, _1, _2, _3)}}}, + {"PERMISSIONS", { + {"permissions.p7s", std::bind(process_file_uri_security_file, _1, _2, _3)}}}, + }; + + const std::unordered_map optional_files{ + {"CRL", { + {"crl.pem", std::bind(process_file_uri_security_file, _1, _2, _3)}}} + }; + + for (const std::pair>> & el : required_files) + { + std::string attribute_value; + bool processed = false; + for (auto & proc : el.second) { + rcpputils::fs::path full_path(secure_root); + full_path /= proc.first; + if (proc.second(prefix, full_path, attribute_value)) { + processed = true; + break; + } + } + if (!processed) { + result.clear(); + return false; + } + result[el.first] = attribute_value; + } + + for (const std::pair & el : optional_files) { + std::string attribute_value; + bool processed = false; + for (auto & proc : el.second) { + rcpputils::fs::path full_path(secure_root); + full_path /= proc.first; + if (proc.second(prefix, full_path, attribute_value)) { + processed = true; + break; + } + } + if (processed) { + result[el.first] = attribute_value; + } + } + + return true; +} + CustomParticipantInfo * rmw_fastrtps_shared_cpp::create_participant( const char * identifier, @@ -318,7 +435,7 @@ rmw_fastrtps_shared_cpp::create_participant( // if security_root_path provided, try to find the key and certificate files #if HAVE_SECURITY std::unordered_map security_files_paths; - if (rmw_dds_common::get_security_files( + if (get_security_files( "file://", security_options->security_root_path, security_files_paths)) { eprosima::fastrtps::rtps::PropertyPolicy property_policy; From 989f3d6ea8d75d0a99b311fc87883147cdaf2573 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:14:58 +0000 Subject: [PATCH 02/20] 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 3c39dd06f94ed226c060c884360c2bd2d7206cd2 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:15:45 +0000 Subject: [PATCH 03/20] 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 80607b8d2e7254d40154a91a39cb64a29d5d6f58 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:20:02 +0000 Subject: [PATCH 04/20] 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 034792d1d3888436355b91804c740f7dbbbfa8a1 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:20:47 +0000 Subject: [PATCH 05/20] 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 eb3d38232a..9cd5653411 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; @@ -129,6 +130,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 94b15688d503d9a128e71b689ec1a470c02edeb0 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:23:47 +0000 Subject: [PATCH 06/20] 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 d041cddfb671b3aeaadfd4821a0c60f05fb4ff94 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:24:50 +0000 Subject: [PATCH 07/20] 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 82f9397f691c4fd58cf16a4fbb799c2dc2910db1 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 11 Jan 2024 15:25:34 +0000 Subject: [PATCH 08/20] 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 899119f887..ce8ad3b884 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; @@ -993,6 +1209,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 1858d24cef7938fc2f3abc1900a8b2047eed3c0e Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Tue, 30 Jan 2024 11:32:50 +0000 Subject: [PATCH 09/20] 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 2b9c5f6d10c3909749b915ef728ff2fc4b275676 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Tue, 30 Jan 2024 11:34:21 +0000 Subject: [PATCH 10/20] 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 9cd5653411..801f23c791 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 @@ -131,13 +150,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) From 5cdfff7349980e9aba5aab5ac551d51fda946a48 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 8 Feb 2024 09:51:15 +0000 Subject: [PATCH 11/20] Refs #20310: rmw_fastrtps_shared_cpp: add abi_version enum and type support member Signed-off-by: Mario Dominguez --- .../include/rmw_fastrtps_shared_cpp/TypeSupport.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) 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 6812842bfa..5ce8d3ffdc 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 @@ -54,6 +54,13 @@ struct SerializedData class TypeSupport : public eprosima::fastdds::dds::TopicDataType { public: + + enum AbiVersion + { + ABI_V1 = 1, + ABI_V2 + }; + virtual size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const = 0; virtual bool serializeROSmessage( @@ -111,6 +118,7 @@ class TypeSupport : public eprosima::fastdds::dds::TopicDataType RMW_FASTRTPS_SHARED_CPP_PUBLIC TypeSupport(); + mutable uint8_t abi_version_; bool max_size_bound_; bool is_plain_; bool key_is_unbounded_; From d74475bf331d5d681b2c1f313c2ebcada77b44e9 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 8 Feb 2024 10:02:13 +0000 Subject: [PATCH 12/20] Refs #20310: rmw_fastrtps_cpp: adopt abi version v2 Signed-off-by: Mario Dominguez --- .../rmw_fastrtps_cpp/MessageTypeSupport.hpp | 2 +- .../include/rmw_fastrtps_cpp/TypeSupport.hpp | 6 +- rmw_fastrtps_cpp/src/publisher.cpp | 4 +- rmw_fastrtps_cpp/src/rmw_serialize.cpp | 8 +- rmw_fastrtps_cpp/src/subscription.cpp | 4 +- rmw_fastrtps_cpp/src/type_support_common.cpp | 80 +++++++++++++------ rmw_fastrtps_cpp/src/type_support_common.hpp | 2 + 7 files changed, 75 insertions(+), 31 deletions(-) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp index 5f431bc46c..7414c90744 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp @@ -25,7 +25,7 @@ namespace rmw_fastrtps_cpp class MessageTypeSupport : public TypeSupport { public: - explicit MessageTypeSupport(const message_type_support_callbacks_t * members); + explicit MessageTypeSupport(const message_type_support_callbacks_t * members, uint8_t abi_version); }; } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp index 762d888017..34d75cf73f 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp @@ -31,6 +31,8 @@ namespace rmw_fastrtps_cpp { +uint8_t get_type_support_abi_version(const char * identifier); + class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport { public: @@ -50,9 +52,11 @@ class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport protected: void set_members(const message_type_support_callbacks_t * members); + void set_members_v2(const message_type_support_callbacks_t * members); + private: const message_type_support_callbacks_t * members_; - message_type_support_key_callbacks_t key_callbacks_; + message_type_support_key_callbacks_t* key_callbacks_; bool has_data_; }; diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index c16db932d2..dae8a58164 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -122,6 +122,8 @@ rmw_fastrtps_cpp::create_publisher( } } + uint8_t abi_version = get_type_support_abi_version(type_support->typesupport_identifier); + std::lock_guard lck(participant_info->entity_creation_mutex_); ///// @@ -176,7 +178,7 @@ rmw_fastrtps_cpp::create_publisher( ///// // Create the Type Support struct if (!fastdds_type) { - auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks, abi_version); if (!tsupport) { RMW_SET_ERROR_MSG("create_publisher() failed to allocate MessageTypeSupport"); return nullptr; diff --git a/rmw_fastrtps_cpp/src/rmw_serialize.cpp b/rmw_fastrtps_cpp/src/rmw_serialize.cpp index 8689e69c50..b0697b7d1c 100644 --- a/rmw_fastrtps_cpp/src/rmw_serialize.cpp +++ b/rmw_fastrtps_cpp/src/rmw_serialize.cpp @@ -40,7 +40,9 @@ rmw_serialize( } auto callbacks = static_cast(ts->data); - auto tss = MessageTypeSupport_cpp(callbacks); + + uint8_t abi_version = rmw_fastrtps_cpp::get_type_support_abi_version(type_support->typesupport_identifier); + auto tss = MessageTypeSupport_cpp(callbacks, abi_version); auto data_length = tss.getEstimatedSerializedSize(ros_message, callbacks); if (serialized_message->buffer_capacity < data_length) { if (rmw_serialized_message_resize(serialized_message, data_length) != RMW_RET_OK) { @@ -78,7 +80,9 @@ rmw_deserialize( } auto callbacks = static_cast(ts->data); - auto tss = MessageTypeSupport_cpp(callbacks); + + uint8_t abi_version = rmw_fastrtps_cpp::get_type_support_abi_version(type_support->typesupport_identifier); + auto tss = MessageTypeSupport_cpp(callbacks, abi_version); eprosima::fastcdr::FastBuffer buffer( reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 2375f45cbd..8448bdac97 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -494,6 +494,8 @@ __create_subscription( } } + uint8_t abi_version = get_type_support_abi_version(type_support->typesupport_identifier); + std::lock_guard lck(participant_info->entity_creation_mutex_); ///// @@ -549,7 +551,7 @@ __create_subscription( ///// // Create the Type Support struct if (!fastdds_type) { - auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks, abi_version); if (!tsupport) { RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); return nullptr; diff --git a/rmw_fastrtps_cpp/src/type_support_common.cpp b/rmw_fastrtps_cpp/src/type_support_common.cpp index 801f23c791..178e5b0201 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_cpp/src/type_support_common.cpp @@ -23,8 +23,22 @@ namespace rmw_fastrtps_cpp { +uint8_t get_type_support_abi_version(const char * identifier) +{ + uint8_t abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V1; + + if (strcmp(identifier, RMW_FASTRTPS_CPP_TYPESUPPORT_C_V2) == 0 || + strcmp(identifier, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP_V2) == 0) + { + abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V2; + } + + return abi_version; +} + TypeSupport::TypeSupport() { + abi_version_ = AbiVersion::ABI_V1; m_isGetKeyDefined = false; max_size_bound_ = false; is_plain_ = false; @@ -35,7 +49,6 @@ TypeSupport::TypeSupport() 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; @@ -56,27 +69,28 @@ void TypeSupport::set_members(const message_type_support_callbacks_t * members) has_data_ = true; } - if (m_isGetKeyDefined) + // Total size is encapsulation size + data size + m_typeSize = 4 + data_size; + // Account for RTPS submessage alignment + m_typeSize = (m_typeSize + 3) & ~3; +} + +void TypeSupport::set_members_v2(const message_type_support_callbacks_t * members) +{ + + set_members(members); + + if (nullptr != members->key_callbacks) { - 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; + this->key_callbacks_ = members->key_callbacks; + m_isGetKeyDefined = true; + + this->key_max_serialized_size_ = this->key_callbacks_->max_serialized_key_size(0, this->key_is_unbounded_); 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 - m_typeSize = (m_typeSize + 3) & ~3; } size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const void * impl) const @@ -166,11 +180,8 @@ bool TypeSupport::getKeyHashFromROSmessage( //! 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_max_serialized_size_ = key_callbacks_->get_serialized_key_size(ros_message, 0); 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( @@ -180,7 +191,7 @@ bool TypeSupport::getKeyHashFromROSmessage( eprosima::fastcdr::Cdr ser( fast_buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - key_callbacks_.cdr_serialize_key(ros_message, ser); + key_callbacks_->cdr_serialize_key(ros_message, ser); //! check for md5 if (force_md5 || this->key_max_serialized_size_ > 16) @@ -209,19 +220,38 @@ bool TypeSupport::getKeyHashFromROSmessage( } } - std::cout << "\nFinishing static::getKeyHashFromROSmessage() " << ihandle->value << std::endl; - return true; } -MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * members) +MessageTypeSupport::MessageTypeSupport( + const message_type_support_callbacks_t * members, + uint8_t abi_version) { assert(members); + abi_version_ = abi_version; + std::string name = _create_type_name(members); this->setName(name.c_str()); - set_members(members); + switch (abi_version) + { + case TypeSupport::AbiVersion::ABI_V1: + { + set_members(members); + break; + } + case TypeSupport::AbiVersion::ABI_V2: + { + set_members_v2(members); + break; + } + default: + { + set_members(members); + break; + } + } } ServiceTypeSupport::ServiceTypeSupport() diff --git a/rmw_fastrtps_cpp/src/type_support_common.hpp b/rmw_fastrtps_cpp/src/type_support_common.hpp index 758f8bed85..645d46f448 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_cpp/src/type_support_common.hpp @@ -33,6 +33,8 @@ #include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" #define RMW_FASTRTPS_CPP_TYPESUPPORT_C rosidl_typesupport_fastrtps_c__identifier #define RMW_FASTRTPS_CPP_TYPESUPPORT_CPP rosidl_typesupport_fastrtps_cpp::typesupport_identifier +#define RMW_FASTRTPS_CPP_TYPESUPPORT_C_V2 rosidl_typesupport_fastrtps_c__identifier_v2 +#define RMW_FASTRTPS_CPP_TYPESUPPORT_CPP_V2 rosidl_typesupport_fastrtps_cpp::typesupport_identifier_v2 using MessageTypeSupport_cpp = rmw_fastrtps_cpp::MessageTypeSupport; using TypeSupport_cpp = rmw_fastrtps_cpp::TypeSupport; From 3c3801b35fd22462698164ad7f96b41caca7eb50 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 8 Feb 2024 10:27:34 +0000 Subject: [PATCH 13/20] Refs #20310: rmw_fastrtps_dynamic_cpp: adopt abi version v2 Signed-off-by: Mario Dominguez --- .../MessageTypeSupport.hpp | 2 +- .../MessageTypeSupport_impl.hpp | 3 +- .../ServiceTypeSupport.hpp | 4 +-- .../ServiceTypeSupport_impl.hpp | 6 ++-- .../TypeSupport_impl.hpp | 11 ++++-- .../src/client_service_common.cpp | 12 ++++--- .../src/type_support_common.cpp | 31 +++++++++++++--- .../src/type_support_common.hpp | 12 ++++--- .../src/type_support_registry.cpp | 36 ++++++++++++------- 9 files changed, 83 insertions(+), 34 deletions(-) diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp index 67d8a72b89..6aa2b8123a 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp @@ -32,7 +32,7 @@ template class MessageTypeSupport : public TypeSupport { public: - MessageTypeSupport(const MembersType * members, const void * ros_type_support); + MessageTypeSupport(const MembersType * members, const void * ros_type_support, uint8_t abi_version); }; } // namespace rmw_fastrtps_dynamic_cpp 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 6e9e11c3f0..16f7540350 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 @@ -33,11 +33,12 @@ namespace rmw_fastrtps_dynamic_cpp template MessageTypeSupport::MessageTypeSupport( - const MembersType * members, const void * ros_type_support) + const MembersType * members, const void * ros_type_support, uint8_t abi_version) : TypeSupport(ros_type_support) { assert(members); this->members_ = members; + this->abi_version_ = abi_version; std::ostringstream ss; std::string message_namespace(this->members_->message_namespace_); diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp index c4ac036d7a..47b2cf4f46 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp @@ -30,14 +30,14 @@ template class RequestTypeSupport : public TypeSupport { public: - RequestTypeSupport(const ServiceMembersType * members, const void * ros_type_support); + RequestTypeSupport(const ServiceMembersType * members, const void * ros_type_support, uint8_t abi_version); }; template class ResponseTypeSupport : public TypeSupport { public: - ResponseTypeSupport(const ServiceMembersType * members, const void * ros_type_support); + ResponseTypeSupport(const ServiceMembersType * members, const void * ros_type_support, uint8_t abi_version); }; } // namespace rmw_fastrtps_dynamic_cpp 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 2eaf321558..e6002fa4e2 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 @@ -32,11 +32,12 @@ namespace rmw_fastrtps_dynamic_cpp template RequestTypeSupport::RequestTypeSupport( - const ServiceMembersType * members, const void * ros_type_support) + const ServiceMembersType * members, const void * ros_type_support, uint8_t abi_version) : TypeSupport(ros_type_support) { assert(members); this->members_ = members->request_members_; + this->abi_version_ = abi_version; std::ostringstream ss; std::string service_namespace(members->service_namespace_); @@ -72,11 +73,12 @@ RequestTypeSupport::RequestTypeSupport( template ResponseTypeSupport::ResponseTypeSupport( - const ServiceMembersType * members, const void * ros_type_support) + const ServiceMembersType * members, const void * ros_type_support, uint8_t abi_version) : TypeSupport(ros_type_support) { assert(members); this->members_ = members->response_members_; + this->abi_version_ = abi_version; std::ostringstream ss; std::string service_namespace(members->service_namespace_); 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 ce8ad3b884..9a6249aea0 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 @@ -311,7 +311,9 @@ bool TypeSupport::serializeKeyROSmessage( 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_) + if (check_if_member_is_key && + this->abi_version_ != AbiVersion::ABI_V1 && + !*(members->key_members_array_+i)) { continue; } @@ -638,7 +640,9 @@ size_t TypeSupport::getEstimatedSerializedSize( const auto member = members->members_ + i; void * field = const_cast(static_cast(ros_message)) + member->offset_; - if (compute_key_members_only && !member->is_key_) + if (compute_key_members_only && + this->abi_version_ != AbiVersion::ABI_V1 && + !*(members->key_members_array_+i)) { continue; } @@ -1000,7 +1004,8 @@ size_t TypeSupport::calculateMaxSerializedSize( size_t array_size = 1; bool member_is_key = false; - if (member->is_key_) + if (this->abi_version_ != AbiVersion::ABI_V1 && + *(members->key_members_array_+i)) { member_is_key = true; } diff --git a/rmw_fastrtps_dynamic_cpp/src/client_service_common.cpp b/rmw_fastrtps_dynamic_cpp/src/client_service_common.cpp index 7677079d34..e77383ed38 100644 --- a/rmw_fastrtps_dynamic_cpp/src/client_service_common.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/client_service_common.cpp @@ -20,10 +20,12 @@ const void * get_request_ptr(const void * untyped_service_members, const char * typesupport) { - if (using_introspection_c_typesupport(typesupport)) { + uint8_t abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V1; + (void)abi_version; + if (using_introspection_c_typesupport(typesupport, abi_version)) { return get_request_ptr( untyped_service_members); - } else if (using_introspection_cpp_typesupport(typesupport)) { + } else if (using_introspection_cpp_typesupport(typesupport, abi_version)) { return get_request_ptr( untyped_service_members); } @@ -34,10 +36,12 @@ get_request_ptr(const void * untyped_service_members, const char * typesupport) const void * get_response_ptr(const void * untyped_service_members, const char * typesupport) { - if (using_introspection_c_typesupport(typesupport)) { + uint8_t abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V1; + (void)abi_version; + if (using_introspection_c_typesupport(typesupport, abi_version)) { return get_response_ptr( untyped_service_members); - } else if (using_introspection_cpp_typesupport(typesupport)) { + } else if (using_introspection_cpp_typesupport(typesupport, abi_version)) { return get_response_ptr( untyped_service_members); } diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp index d60206970a..c5fb722800 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp @@ -23,14 +23,35 @@ #include "type_support_common.hpp" bool -using_introspection_c_typesupport(const char * typesupport_identifier) +using_introspection_c_typesupport(const char * typesupport_identifier, uint8_t &abi_version) { - return typesupport_identifier == rosidl_typesupport_introspection_c__identifier; + bool ret = false; + if (strcmp(typesupport_identifier, rosidl_typesupport_introspection_c__identifier) == 0) + { + abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V1; + ret = true; + + } else if (strcmp(typesupport_identifier, rosidl_typesupport_introspection_c__identifier_v2) == 0) + { + abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V2; + ret = true; + } + return ret; } bool -using_introspection_cpp_typesupport(const char * typesupport_identifier) +using_introspection_cpp_typesupport(const char * typesupport_identifier, uint8_t &abi_version) { - return typesupport_identifier == - rosidl_typesupport_introspection_cpp::typesupport_identifier; + bool ret = false; + if (strcmp(typesupport_identifier, rosidl_typesupport_introspection_cpp::typesupport_identifier) == 0) + { + abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V1; + ret = true; + + } else if (strcmp(typesupport_identifier, rosidl_typesupport_introspection_cpp::typesupport_identifier_v2) == 0) + { + abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V2; + ret = true; + } + return ret; } diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp index 1846b7ba5d..4dc6e05ea3 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp @@ -63,10 +63,12 @@ using ResponseTypeSupport_cpp = rmw_fastrtps_dynamic_cpp::ResponseTypeSupport< >; bool -using_introspection_c_typesupport(const char * typesupport_identifier); +using_introspection_c_typesupport(const char * typesupport_identifier, + uint8_t& abi_version); bool -using_introspection_cpp_typesupport(const char * typesupport_identifier); +using_introspection_cpp_typesupport(const char * typesupport_identifier, + uint8_t& abi_version); template ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL @@ -98,10 +100,12 @@ _create_type_name( const void * untyped_members, const char * typesupport) { - if (using_introspection_c_typesupport(typesupport)) { + uint8_t abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V1; + (void)abi_version; + if (using_introspection_c_typesupport(typesupport, abi_version)) { return _create_type_name( untyped_members); - } else if (using_introspection_cpp_typesupport(typesupport)) { + } else if (using_introspection_cpp_typesupport(typesupport, abi_version)) { return _create_type_name( untyped_members); } diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp index d7f635b847..5d2092a956 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp @@ -81,14 +81,18 @@ type_support_ptr TypeSupportRegistry::get_message_type_support( { auto creator_fun = [&ros_type_support]() -> type_support_ptr { - if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier)) { + uint8_t abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V1; + if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier, abi_version)) + { auto members = static_cast( ros_type_support->data); - return new MessageTypeSupport_c(members, ros_type_support); - } else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier)) { + return new MessageTypeSupport_c(members, ros_type_support, abi_version); + } + else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier, abi_version)) + { auto members = static_cast( ros_type_support->data); - return new MessageTypeSupport_cpp(members, ros_type_support); + return new MessageTypeSupport_cpp(members, ros_type_support, abi_version); } RMW_SET_ERROR_MSG("Unknown typesupport identifier"); return nullptr; @@ -102,14 +106,18 @@ type_support_ptr TypeSupportRegistry::get_request_type_support( { auto creator_fun = [&ros_type_support]() -> type_support_ptr { - if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier)) { + uint8_t abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V1; + if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier, abi_version)) + { auto members = static_cast( ros_type_support->data); - return new RequestTypeSupport_c(members, ros_type_support); - } else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier)) { + return new RequestTypeSupport_c(members, ros_type_support, abi_version); + } + else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier, abi_version)) + { auto members = static_cast( ros_type_support->data); - return new RequestTypeSupport_cpp(members, ros_type_support); + return new RequestTypeSupport_cpp(members, ros_type_support, abi_version); } RMW_SET_ERROR_MSG("Unknown typesupport identifier"); return nullptr; @@ -123,14 +131,18 @@ type_support_ptr TypeSupportRegistry::get_response_type_support( { auto creator_fun = [&ros_type_support]() -> type_support_ptr { - if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier)) { + uint8_t abi_version = rmw_fastrtps_shared_cpp::TypeSupport::AbiVersion::ABI_V1; + if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier, abi_version)) + { auto members = static_cast( ros_type_support->data); - return new ResponseTypeSupport_c(members, ros_type_support); - } else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier)) { + return new ResponseTypeSupport_c(members, ros_type_support, abi_version); + } + else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier, abi_version)) + { auto members = static_cast( ros_type_support->data); - return new ResponseTypeSupport_cpp(members, ros_type_support); + return new ResponseTypeSupport_cpp(members, ros_type_support, abi_version); } RMW_SET_ERROR_MSG("Unknown typesupport identifier"); return nullptr; From 1bf49f501f146abf38094ba88cbc8cac5117e544 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 8 Feb 2024 10:28:58 +0000 Subject: [PATCH 14/20] Refs #20310: rmw_fastrtps_shared_cpp: apply_qos_resource_limits_for_keys() free function Signed-off-by: Mario Dominguez --- .../include/rmw_fastrtps_shared_cpp/utils.hpp | 16 ++++++++++++++++ rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp | 8 ++++++++ rmw_fastrtps_shared_cpp/src/utils.cpp | 16 ++++++++++++++++ 3 files changed, 40 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp index 67f1a5ad6d..1ef9c6a227 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -126,6 +126,22 @@ create_datareader( CustomDataReaderListener * listener, eprosima::fastdds::dds::DataReader ** data_reader); + +/** +* Apply specific resource limits when using keys. +* Max samples per instance is set to history depth if KEEP_LAST +* else UNLIMITED. +* +* \param[in] hitory_qos History entitiy QoS. +* \param[in, out] res_limits_qos Resource limits entitiy QoS. +* +*/ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +void +apply_qos_resource_limits_for_keys( + const eprosima::fastdds::dds::HistoryQosPolicy & history_qos, + eprosima::fastdds::dds::ResourceLimitsQosPolicy & res_limits_qos); + } // namespace rmw_fastrtps_shared_cpp #endif // RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 218b0ff381..624f3dd196 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -167,6 +167,14 @@ __rmw_subscription_set_content_filter( des_topic = info->topic_; } + /// Apply resource limits QoS if the type is keyed + if (info->type_support_->m_isGetKeyDefined) + { + apply_qos_resource_limits_for_keys( + info->datareader_qos_.history(), + info->datareader_qos_.resource_limits()); + } + // create data reader eprosima::fastdds::dds::Subscriber * subscriber = info->subscriber_; const rmw_subscription_options_t * subscription_options = diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index 9e72e9030a..c71f3f9ec5 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -180,5 +180,21 @@ create_datareader( return true; } +void +apply_qos_resource_limits_for_keys( + const eprosima::fastdds::dds::HistoryQosPolicy & history_qos, + eprosima::fastdds::dds::ResourceLimitsQosPolicy & res_limits_qos) +{ + res_limits_qos.max_instances = 0; + res_limits_qos.max_samples = 0; + if (history_qos.kind == eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS) + { + res_limits_qos.max_samples_per_instance = history_qos.depth; + } + else + { + res_limits_qos.max_samples_per_instance = 0; + } +} } // namespace rmw_fastrtps_shared_cpp From 6bc210f3a56cc75963e1ef6bf1b2a9a14aec622d Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 8 Feb 2024 10:33:21 +0000 Subject: [PATCH 15/20] Refs #20310: rmw_fastrtps_cpp: apply qos for keys Signed-off-by: Mario Dominguez --- rmw_fastrtps_cpp/src/publisher.cpp | 8 ++++++++ rmw_fastrtps_cpp/src/rmw_client.cpp | 16 ++++++++++++++++ rmw_fastrtps_cpp/src/rmw_service.cpp | 16 ++++++++++++++++ rmw_fastrtps_cpp/src/subscription.cpp | 16 ++++++++++++++++ 4 files changed, 56 insertions(+) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index dae8a58164..12f106140a 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -266,6 +266,14 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + writer_qos.history(), + writer_qos.resource_limits()); + } + // Creates DataWriter with a mask enabling publication_matched calls for the listener info->data_writer_ = publisher->create_datawriter( info->topic_, diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index d94ae48596..c60f994733 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -323,6 +323,14 @@ rmw_create_client( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (response_fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + reader_qos.history(), + reader_qos.resource_limits()); + } + // Creates DataReader info->response_reader_ = subscriber->create_datareader( response_topic_desc, @@ -381,6 +389,14 @@ rmw_create_client( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (request_fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + writer_qos.history(), + writer_qos.resource_limits()); + } + // Creates DataWriter with a mask enabling publication_matched calls for the listener info->request_writer_ = publisher->create_datawriter( info->request_topic_, diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 118b4e2f9d..cb3eeea89f 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -322,6 +322,14 @@ rmw_create_service( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (request_fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + reader_qos.history(), + reader_qos.resource_limits()); + } + // Creates DataReader info->request_reader_ = subscriber->create_datareader( request_topic_desc, @@ -384,6 +392,14 @@ rmw_create_service( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (response_fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + writer_qos.history(), + writer_qos.resource_limits()); + } + // Creates DataWriter with a mask enabling publication_matched calls for the listener info->response_writer_ = publisher->create_datawriter( info->response_topic_, diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 8448bdac97..0410476771 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -392,6 +392,14 @@ __create_dynamic_subscription( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + info->datareader_qos_.history(), + info->datareader_qos_.resource_limits()); + } + info->datareader_qos_ = reader_qos; // create_datareader @@ -661,6 +669,14 @@ __create_subscription( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + reader_qos.history(), + reader_qos.resource_limits()); + } + info->datareader_qos_ = reader_qos; // create_datareader From 02e367ce0fd0baf95f54ab73e252d4253a95634e Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 8 Feb 2024 10:35:28 +0000 Subject: [PATCH 16/20] Refs #20310: rmw_fastrtps_dynamic_cpp: apply qos for keys Signed-off-by: Mario Dominguez --- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 8 ++++++++ rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 16 ++++++++++++++++ rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 16 ++++++++++++++++ rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 8 ++++++++ 4 files changed, 48 insertions(+) diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 0135129e9d..510730bc71 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -270,6 +270,14 @@ rmw_fastrtps_dynamic_cpp::create_publisher( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + writer_qos.history(), + writer_qos.resource_limits()); + } + // Creates DataWriter (with publisher name to not change name policy) info->data_writer_ = publisher->create_datawriter( info->topic_, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index d8626b54cf..c4d2a4eb5a 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -354,6 +354,14 @@ rmw_create_client( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (response_fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + reader_qos.history(), + reader_qos.resource_limits()); + } + // Creates DataReader info->response_reader_ = subscriber->create_datareader( response_topic_desc, @@ -412,6 +420,14 @@ rmw_create_client( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (request_fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + writer_qos.history(), + writer_qos.resource_limits()); + } + // Creates DataWriter info->request_writer_ = publisher->create_datawriter( info->request_topic_, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 4e1fe8341c..b073170801 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -353,6 +353,14 @@ rmw_create_service( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (request_fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + reader_qos.history(), + reader_qos.resource_limits()); + } + // Creates DataReader info->request_reader_ = subscriber->create_datareader( request_topic_desc, @@ -415,6 +423,14 @@ rmw_create_service( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (response_fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + writer_qos.history(), + writer_qos.resource_limits()); + } + // Creates DataWriter info->response_writer_ = publisher->create_datawriter( info->response_topic_, diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 1c8630e215..ff8292b88f 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -274,6 +274,14 @@ create_subscription( return nullptr; } + /// Apply resource limits QoS if the type is keyed + if (fastdds_type->m_isGetKeyDefined) + { + rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( + reader_qos.history(), + reader_qos.resource_limits()); + } + eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; switch (subscription_options->require_unique_network_flow_endpoints) { default: From c2fa7796c4ef1a448261a65b53b237f9cb3477f6 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 11:14:50 +0000 Subject: [PATCH 17/20] Refs #20310: Review suggestions Signed-off-by: Mario Dominguez --- .../include/rmw_fastrtps_cpp/TypeSupport.hpp | 4 +- rmw_fastrtps_cpp/src/publisher.cpp | 3 +- rmw_fastrtps_cpp/src/rmw_client.cpp | 6 +- rmw_fastrtps_cpp/src/rmw_service.cpp | 6 +- rmw_fastrtps_cpp/src/subscription.cpp | 6 +- rmw_fastrtps_cpp/src/type_support_common.cpp | 56 +++++++++---------- .../rmw_fastrtps_dynamic_cpp/TypeSupport.hpp | 6 +- .../TypeSupport_impl.hpp | 6 +- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 3 +- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 6 +- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 6 +- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 3 +- .../src/type_support_proxy.cpp | 4 +- .../rmw_fastrtps_shared_cpp/TypeSupport.hpp | 2 +- .../src/TypeSupport_impl.cpp | 15 ++++- .../src/rmw_subscription.cpp | 8 --- 16 files changed, 77 insertions(+), 63 deletions(-) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp index 34d75cf73f..34adbefe3f 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp @@ -44,7 +44,7 @@ class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport bool deserializeROSmessage( eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; - bool getKeyHashFromROSmessage( + bool get_key_hash_from_ros_message( void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, const void * impl) const override; TypeSupport(); @@ -56,7 +56,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_; + const message_type_support_key_callbacks_t* key_callbacks_; bool has_data_; }; diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 12f106140a..9434c4d7aa 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -267,7 +267,8 @@ rmw_fastrtps_cpp::create_publisher( } /// Apply resource limits QoS if the type is keyed - if (fastdds_type->m_isGetKeyDefined) + if (fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( writer_qos.history(), diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index c60f994733..9edef0b75a 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -324,7 +324,8 @@ rmw_create_client( } /// Apply resource limits QoS if the type is keyed - if (response_fastdds_type->m_isGetKeyDefined) + if (response_fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( reader_qos.history(), @@ -390,7 +391,8 @@ rmw_create_client( } /// Apply resource limits QoS if the type is keyed - if (request_fastdds_type->m_isGetKeyDefined) + if (request_fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( writer_qos.history(), diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index cb3eeea89f..9afb1ba416 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -323,7 +323,8 @@ rmw_create_service( } /// Apply resource limits QoS if the type is keyed - if (request_fastdds_type->m_isGetKeyDefined) + if (request_fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( reader_qos.history(), @@ -393,7 +394,8 @@ rmw_create_service( } /// Apply resource limits QoS if the type is keyed - if (response_fastdds_type->m_isGetKeyDefined) + if (response_fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( writer_qos.history(), diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 0410476771..4899a7a611 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -393,7 +393,8 @@ __create_dynamic_subscription( } /// Apply resource limits QoS if the type is keyed - if (fastdds_type->m_isGetKeyDefined) + if (fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( info->datareader_qos_.history(), @@ -670,7 +671,8 @@ __create_subscription( } /// Apply resource limits QoS if the type is keyed - if (fastdds_type->m_isGetKeyDefined) + if (fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( reader_qos.history(), diff --git a/rmw_fastrtps_cpp/src/type_support_common.cpp b/rmw_fastrtps_cpp/src/type_support_common.cpp index 178e5b0201..d1867e0eec 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_cpp/src/type_support_common.cpp @@ -44,6 +44,9 @@ TypeSupport::TypeSupport() is_plain_ = false; key_is_unbounded_ = false; key_max_serialized_size_ = 0; + members_ = nullptr; + key_callbacks_ = nullptr; + has_data_ = false; } void TypeSupport::set_members(const message_type_support_callbacks_t * members) @@ -82,13 +85,13 @@ void TypeSupport::set_members_v2(const message_type_support_callbacks_t * member if (nullptr != members->key_callbacks) { - this->key_callbacks_ = members->key_callbacks; + key_callbacks_ = members->key_callbacks; m_isGetKeyDefined = true; - this->key_max_serialized_size_ = this->key_callbacks_->max_serialized_key_size(0, this->key_is_unbounded_); - if (!this->key_is_unbounded_) + key_max_serialized_size_ = key_callbacks_->max_serialized_size_key(0, key_is_unbounded_); + if (!key_is_unbounded_) { - this->key_buffer_.reserve(this->key_max_serialized_size_); + key_buffer_.reserve(key_max_serialized_size_); } } } @@ -163,7 +166,7 @@ bool TypeSupport::deserializeROSmessage( return true; } -bool TypeSupport::getKeyHashFromROSmessage( +bool TypeSupport::get_key_hash_from_ros_message( void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, @@ -172,51 +175,48 @@ bool TypeSupport::getKeyHashFromROSmessage( assert(ros_message); (void)impl; - /*if (capacity < 16) + // retrieve estimated serialized size in case key is unbounded + if (key_is_unbounded_) { - throw std::runtime_error("Not enough capacity to serialize key"); - }*/ - - //! retrieve estimated serialized size in case key is unbounded - if (this->key_is_unbounded_) - { - this->key_max_serialized_size_ = key_callbacks_->get_serialized_key_size(ros_message, 0); - this->key_buffer_.reserve(this->key_max_serialized_size_); + key_max_serialized_size_ = (std::max) (key_callbacks_->get_serialized_size_key(ros_message, 0), (size_t)0); + key_buffer_.reserve(key_max_serialized_size_); } eprosima::fastcdr::FastBuffer fast_buffer( - reinterpret_cast(this->key_buffer_.data()), - this->key_max_serialized_size_); + reinterpret_cast(key_buffer_.data()), + 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) - { + const size_t max_serialized_key_length = 16; - this->md5_.init(); - -#if FASTCDR_VERSION_MAJOR == 1 - this->md5_.update(this->key_buffer_.data(), static_cast(ser.getSerializedDataLength())); + #if FASTCDR_VERSION_MAJOR == 1 + auto ser_length = ser.getSerializedDataLength(); #else - this->md5_.update(this->key_buffer_.data(), static_cast(ser.get_serialized_data_length())); + auto ser_length = ser.get_serialized_data_length(); #endif // FASTCDR_VERSION_MAJOR == 1 - this->md5_.finalize(); + // check for md5 + if (force_md5 || key_max_serialized_size_ > max_serialized_key_length) + { + md5_.init(); + md5_.update(key_buffer_.data(), static_cast(ser_length)); + md5_.finalize(); - for (uint8_t i = 0; i < 16; ++i) + for (uint8_t i = 0; i < max_serialized_key_length; ++i) { ihandle->value[i] = md5_.digest[i]; } } else { - for (uint8_t i = 0; i < 16; ++i) + memset(ihandle->value, 0, max_serialized_key_length); + for (uint8_t i = 0; i < ser_length; ++i) { - ihandle->value[i] = this->key_buffer_[i]; + ihandle->value[i] = key_buffer_[i]; } } 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 84142f41c2..eb575a3e97 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 @@ -139,7 +139,7 @@ class TypeSupportProxy : public rmw_fastrtps_shared_cpp::TypeSupport bool deserializeROSmessage( eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; - bool getKeyHashFromROSmessage( + bool get_key_hash_from_ros_message( void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, const void * impl) const override; }; @@ -174,7 +174,7 @@ class TypeSupport : public BaseTypeSupport bool deserializeROSmessage( eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; - bool getKeyHashFromROSmessage( + bool get_key_hash_from_ros_message( void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, const void * impl) const override; protected: @@ -207,7 +207,7 @@ class TypeSupport : public BaseTypeSupport void * ros_message, bool check_if_member_is_key) const; - bool getKeyHashFromROSmessage( + bool get_key_hash_from_ros_message( const MembersType * members, void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, 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 9a6249aea0..dd484bbc9e 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 @@ -407,7 +407,7 @@ bool TypeSupport::serializeKeyROSmessage( } template -bool TypeSupport::getKeyHashFromROSmessage( +bool TypeSupport::get_key_hash_from_ros_message( const MembersType * members, void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, @@ -1215,7 +1215,7 @@ bool TypeSupport::deserializeROSmessage( } template -bool TypeSupport::getKeyHashFromROSmessage( +bool TypeSupport::get_key_hash_from_ros_message( void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, const void * impl) const { @@ -1227,7 +1227,7 @@ bool TypeSupport::getKeyHashFromROSmessage( (void)impl; if (members_->member_count_ != 0) { - ret = TypeSupport::getKeyHashFromROSmessage(members_, ros_message, ihandle, force_md5); + ret = TypeSupport::get_key_hash_from_ros_message(members_, ros_message, ihandle, force_md5); } return ret; diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 510730bc71..d8b895f6f6 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -271,7 +271,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } /// Apply resource limits QoS if the type is keyed - if (fastdds_type->m_isGetKeyDefined) + if (fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( writer_qos.history(), diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index c4d2a4eb5a..60daf618e6 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -355,7 +355,8 @@ rmw_create_client( } /// Apply resource limits QoS if the type is keyed - if (response_fastdds_type->m_isGetKeyDefined) + if (response_fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( reader_qos.history(), @@ -421,7 +422,8 @@ rmw_create_client( } /// Apply resource limits QoS if the type is keyed - if (request_fastdds_type->m_isGetKeyDefined) + if (request_fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( writer_qos.history(), diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index b073170801..ddb4b38f2f 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -354,7 +354,8 @@ rmw_create_service( } /// Apply resource limits QoS if the type is keyed - if (request_fastdds_type->m_isGetKeyDefined) + if (request_fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( reader_qos.history(), @@ -424,7 +425,8 @@ rmw_create_service( } /// Apply resource limits QoS if the type is keyed - if (response_fastdds_type->m_isGetKeyDefined) + if (response_fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( writer_qos.history(), diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index ff8292b88f..260a3bc849 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -275,7 +275,8 @@ create_subscription( } /// Apply resource limits QoS if the type is keyed - if (fastdds_type->m_isGetKeyDefined) + if (fastdds_type->m_isGetKeyDefined && + !participant_info->leave_middleware_default_qos) { rmw_fastrtps_shared_cpp::apply_qos_resource_limits_for_keys( reader_qos.history(), diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp index 14a478d127..86312756c7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp @@ -47,11 +47,11 @@ bool TypeSupportProxy::deserializeROSmessage( return type_impl->deserializeROSmessage(deser, ros_message, impl); } -bool TypeSupportProxy::getKeyHashFromROSmessage( +bool TypeSupportProxy::get_key_hash_from_ros_message( 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); + return type_impl->get_key_hash_from_ros_message(ros_message, ihandle, force_md5, impl); } } // namespace rmw_fastrtps_dynamic_cpp 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 5ce8d3ffdc..cc844ff851 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 @@ -69,7 +69,7 @@ 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( + virtual bool get_key_hash_from_ros_message( void * ros_message, eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, bool force_md5, const void * impl) const = 0; RMW_FASTRTPS_SHARED_CPP_PUBLIC diff --git a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp index 05766ea56c..1e746e13f4 100644 --- a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -84,19 +84,28 @@ bool TypeSupport::getKey( { case FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE: { - ret = this->getKeyHashFromROSmessage(ser_data->data, ihandle, force_md5, ser_data->impl); + ret = this->get_key_hash_from_ros_message(ser_data->data, ihandle, force_md5, ser_data->impl); break; } case FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER: { - //! TODO + // TODO + // We would need a get_key_hash_from_payload method break; } case FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE: { - //! TODO + + auto m_type = std::make_shared(); + + // Retrieves the key (ihandle) from the dynamic data stored in data->data + return m_type->getKey( + static_cast(ser_data->data), + ihandle, + force_md5); + break; } default: diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 624f3dd196..218b0ff381 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -167,14 +167,6 @@ __rmw_subscription_set_content_filter( des_topic = info->topic_; } - /// Apply resource limits QoS if the type is keyed - if (info->type_support_->m_isGetKeyDefined) - { - apply_qos_resource_limits_for_keys( - info->datareader_qos_.history(), - info->datareader_qos_.resource_limits()); - } - // create data reader eprosima::fastdds::dds::Subscriber * subscriber = info->subscriber_; const rmw_subscription_options_t * subscription_options = From 914358ab23750b984d9b2a1fbdd6c5a0dc958f1b Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 11:23:26 +0000 Subject: [PATCH 18/20] Refs #20310: fix: include checking for the v2 identifier when trying to register the typesupport_introspection. This is useful when builtin types are also built with v2 abi and rmw_fastrtps_cpp is used Signed-off-by: Mario Dominguez --- .../src/TypeSupport_impl.cpp | 30 +++++++++++++------ 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp index 1e746e13f4..61ed8fd881 100644 --- a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -258,15 +258,27 @@ get_type_support_introspection(const rosidl_message_type_support_t * type_suppor type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); if (nullptr == type_support) { - rcutils_error_string_t error_string = rcutils_get_error_string(); - rcutils_reset_error(); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Type support not from this implementation. Got:\n" - " %s\n" - " %s\n" - "while fetching it", - prev_error_string.str, error_string.str); - return nullptr; + type_support = + get_message_typesupport_handle( + type_supports, + rosidl_typesupport_introspection_c__identifier_v2); + if (nullptr == type_support) { + type_support = + get_message_typesupport_handle( + type_supports, + rosidl_typesupport_introspection_cpp::typesupport_identifier_v2); + if (nullptr == type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; + } + } } } From ba0eda9d446d6f91a1918ec702051a368774d46b Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Tue, 27 Feb 2024 16:25:44 +0000 Subject: [PATCH 19/20] Refs #20310: rmw_fastrtps_cpp review 2 suggestion Signed-off-by: Mario Dominguez --- rmw_fastrtps_cpp/src/type_support_common.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rmw_fastrtps_cpp/src/type_support_common.cpp b/rmw_fastrtps_cpp/src/type_support_common.cpp index d1867e0eec..c7149a1500 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_cpp/src/type_support_common.cpp @@ -178,7 +178,9 @@ bool TypeSupport::get_key_hash_from_ros_message( // retrieve estimated serialized size in case key is unbounded if (key_is_unbounded_) { - key_max_serialized_size_ = (std::max) (key_callbacks_->get_serialized_size_key(ros_message, 0), (size_t)0); + key_max_serialized_size_ = (std::max) ( + key_max_serialized_size_, + key_callbacks_->get_serialized_size_key(ros_message, 0)); key_buffer_.reserve(key_max_serialized_size_); } From 4462d31da10338a45d45552227463ca07f1b11a2 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Wed, 28 Feb 2024 15:15:46 +0000 Subject: [PATCH 20/20] Refs #20310: Updates according latest message_type_support structure (no initial alignment) Signed-off-by: Mario Dominguez --- rmw_fastrtps_cpp/src/type_support_common.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_cpp/src/type_support_common.cpp b/rmw_fastrtps_cpp/src/type_support_common.cpp index c7149a1500..b44ae5ccd6 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_cpp/src/type_support_common.cpp @@ -88,7 +88,7 @@ void TypeSupport::set_members_v2(const message_type_support_callbacks_t * member key_callbacks_ = members->key_callbacks; m_isGetKeyDefined = true; - key_max_serialized_size_ = key_callbacks_->max_serialized_size_key(0, key_is_unbounded_); + key_max_serialized_size_ = key_callbacks_->max_serialized_size_key(key_is_unbounded_); if (!key_is_unbounded_) { key_buffer_.reserve(key_max_serialized_size_); @@ -180,7 +180,7 @@ bool TypeSupport::get_key_hash_from_ros_message( { key_max_serialized_size_ = (std::max) ( key_max_serialized_size_, - key_callbacks_->get_serialized_size_key(ros_message, 0)); + key_callbacks_->get_serialized_size_key(ros_message)); key_buffer_.reserve(key_max_serialized_size_); }