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 54fca91e5..0c09766fd 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 @@ -100,7 +100,7 @@ struct StringHelper return std::string(data.data); } - static void assign(eprosima::fastcdr::Cdr & deser, void * field, bool) + static void assign(eprosima::fastcdr::Cdr & deser, void * field) { std::string str; deser >> str; @@ -120,12 +120,9 @@ struct StringHelper return *(static_cast(data)); } - static void assign(eprosima::fastcdr::Cdr & deser, void * field, bool call_new) + static void assign(eprosima::fastcdr::Cdr & deser, void * field) { std::string & str = *(std::string *)field; - if (call_new) { - new(&str) std::string; - } deser >> str; } }; @@ -195,8 +192,7 @@ class TypeSupport : public BaseTypeSupport bool deserializeROSmessage( eprosima::fastcdr::Cdr & deser, const MembersType * members, - void * ros_message, - bool call_new) const; + void * ros_message) const; }; } // namespace rmw_fastrtps_dynamic_cpp 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 551f5f89c..da919d77d 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 @@ -23,6 +23,9 @@ #include "rmw_fastrtps_dynamic_cpp/TypeSupport.hpp" #include "rmw_fastrtps_dynamic_cpp/macros.hpp" + +#include "rmw/error_handling.h" + #include "rosidl_typesupport_fastrtps_c/wstring_conversion.hpp" #include "rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp" #include "rosidl_typesupport_introspection_cpp/field_types.hpp" @@ -55,58 +58,6 @@ SPECIALIZE_GENERIC_C_SEQUENCE(uint32, uint32_t) SPECIALIZE_GENERIC_C_SEQUENCE(int64, int64_t) SPECIALIZE_GENERIC_C_SEQUENCE(uint64, uint64_t) -typedef struct rosidl_runtime_c__void__Sequence -{ - void * data; - /// The number of valid items in data - size_t size; - /// The number of allocated items in data - size_t capacity; -} rosidl_runtime_c__void__Sequence; - -inline -bool -rosidl_runtime_c__void__Sequence__init( - rosidl_runtime_c__void__Sequence * sequence, size_t size, size_t member_size) -{ - if (!sequence) { - return false; - } - void * data = nullptr; - if (size) { - data = static_cast(calloc(size, member_size)); - if (!data) { - return false; - } - } - sequence->data = data; - sequence->size = size; - sequence->capacity = size; - return true; -} - -inline -void -rosidl_runtime_c__void__Sequence__fini(rosidl_runtime_c__void__Sequence * sequence) -{ - if (!sequence) { - return; - } - if (sequence->data) { - // ensure that data and capacity values are consistent - assert(sequence->capacity > 0); - // finalize all sequence elements - free(sequence->data); - sequence->data = nullptr; - sequence->size = 0; - sequence->capacity = 0; - } else { - // ensure that data, size, and capacity values are consistent - assert(0 == sequence->size); - assert(0 == sequence->capacity); - } -} - template TypeSupport::TypeSupport(const void * ros_type_support) : BaseTypeSupport(ros_type_support) @@ -115,99 +66,6 @@ TypeSupport::TypeSupport(const void * ros_type_support) max_size_bound_ = false; } -static inline void * -align_(size_t __align, void * & __ptr) noexcept -{ - const auto __intptr = reinterpret_cast(__ptr); - const auto __aligned = (__intptr - 1u + __align) & ~(__align - 1); - return __ptr = reinterpret_cast(__aligned); -} - -template -static size_t calculateMaxAlign(const MembersType * members) -{ - size_t max_align = 0; - - for (uint32_t i = 0; i < members->member_count_; ++i) { - size_t alignment = 0; - const auto & member = members->members_[i]; - - if (member.is_array_ && (!member.array_size_ || member.is_upper_bound_)) { - alignment = alignof(std::vector); - } else { - switch (member.type_id_) { - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: - alignment = alignof(bool); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: - alignment = alignof(uint8_t); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: - alignment = alignof(char); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: - alignment = alignof(float); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: - alignment = alignof(double); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: - alignment = alignof(int16_t); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: - alignment = alignof(uint16_t); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: - alignment = alignof(int32_t); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: - alignment = alignof(uint32_t); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: - alignment = alignof(int64_t); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: - alignment = alignof(uint64_t); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: - // Note: specialization needed because calculateMaxAlign is called before - // casting submembers as std::string, returned value is the same on i386 - if (std::is_same::value) - { - alignment = alignof(rosidl_runtime_c__String); - } else { - alignment = alignof(std::string); - } - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: - if (std::is_same::value) - { - alignment = alignof(rosidl_runtime_c__U16String); - } else { - alignment = alignof(std::u16string); - } - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: - { - auto sub_members = (const MembersType *)member.members_->data; - alignment = calculateMaxAlign(sub_members); - } - break; - } - } - - if (alignment > max_align) { - max_align = alignment; - } - } - - return max_align; -} - // C++ specialization template void serialize_field( @@ -338,37 +196,31 @@ void serialize_field( } } } + inline -size_t get_array_size_and_assign_field( +void * get_subros_message( const rosidl_typesupport_introspection_cpp::MessageMember * member, void * field, - void * & subros_message, - size_t sub_members_size, - size_t max_align) + size_t index, + size_t, + bool) { - auto vector = reinterpret_cast *>(field); - void * ptr = reinterpret_cast(sub_members_size); - size_t vsize = vector->size() / reinterpret_cast(align_(max_align, ptr)); - if (member->is_upper_bound_ && vsize > member->array_size_) { - throw std::runtime_error("vector overcomes the maximum length"); - } - subros_message = reinterpret_cast(vector->data()); - return vsize; + return member->get_function(field, index); } inline -size_t get_array_size_and_assign_field( +void * get_subros_message( const rosidl_typesupport_introspection_c__MessageMember * member, void * field, - void * & subros_message, - size_t, size_t) + size_t index, + size_t array_size, + bool is_upper_bound) { - auto tmpsequence = static_cast(field); - if (member->is_upper_bound_ && tmpsequence->size > member->array_size_) { - throw std::runtime_error("vector overcomes the maximum length"); + if (array_size && !is_upper_bound) { + return member->get_function(&field, index); } - subros_message = reinterpret_cast(tmpsequence->data); - return tmpsequence->size; + + return member->get_function(field, index); } template @@ -437,26 +289,31 @@ bool TypeSupport::serializeROSmessage( if (!member->is_array_) { serializeROSmessage(ser, sub_members, field); } else { - void * subros_message = nullptr; size_t array_size = 0; - size_t sub_members_size = sub_members->size_of_; - size_t max_align = calculateMaxAlign(sub_members); if (member->array_size_ && !member->is_upper_bound_) { - subros_message = field; array_size = member->array_size_; } else { - array_size = get_array_size_and_assign_field( - member, field, subros_message, sub_members_size, max_align); + 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) { - serializeROSmessage(ser, sub_members, subros_message); - subros_message = static_cast(subros_message) + sub_members_size; - subros_message = align_(max_align, subros_message); + serializeROSmessage( + ser, sub_members, + get_subros_message( + member, field, index, member->array_size_, + member->is_upper_bound_)); } } } @@ -693,27 +550,32 @@ size_t TypeSupport::getEstimatedSerializedSize( if (!member->is_array_) { current_alignment += getEstimatedSerializedSize(sub_members, field, current_alignment); } else { - void * subros_message = nullptr; size_t array_size = 0; - size_t sub_members_size = sub_members->size_of_; - size_t max_align = calculateMaxAlign(sub_members); if (member->array_size_ && !member->is_upper_bound_) { - subros_message = field; array_size = member->array_size_; } else { - array_size = get_array_size_and_assign_field( - member, field, subros_message, sub_members_size, max_align); + if (!member->size_function) { + RMW_SET_ERROR_MSG("unexpected error: size function is null"); + return false; + } + array_size = member->size_function(field); // Length serialization current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); } + 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) { current_alignment += getEstimatedSerializedSize( - sub_members, subros_message, current_alignment); - subros_message = static_cast(subros_message) + sub_members_size; - subros_message = align_(max_align, subros_message); + sub_members, + get_subros_message( + member, field, index, member->array_size_, + member->is_upper_bound_), + current_alignment); } } } @@ -730,8 +592,7 @@ template void deserialize_field( const rosidl_typesupport_introspection_cpp::MessageMember * member, void * field, - eprosima::fastcdr::Cdr & deser, - bool call_new) + eprosima::fastcdr::Cdr & deser) { if (!member->is_array_) { deser >> *static_cast(field); @@ -739,9 +600,6 @@ void deserialize_field( deser.deserializeArray(static_cast(field), member->array_size_); } else { auto & vector = *reinterpret_cast *>(field); - if (call_new) { - new(&vector) std::vector; - } deser >> vector; } } @@ -750,30 +608,15 @@ template<> inline void deserialize_field( const rosidl_typesupport_introspection_cpp::MessageMember * member, void * field, - eprosima::fastcdr::Cdr & deser, - bool call_new) + eprosima::fastcdr::Cdr & deser) { if (!member->is_array_) { - if (call_new) { - // Because std::string is a complex datatype, we need to make sure that - // the memory is initialized to something reasonable before eventually - // passing it as a reference to Fast-CDR. - new(field) std::string(); - } deser >> *static_cast(field); } else if (member->array_size_ && !member->is_upper_bound_) { std::string * array = static_cast(field); - if (call_new) { - for (size_t i = 0; i < member->array_size_; ++i) { - new(&array[i]) std::string(); - } - } deser.deserializeArray(array, member->array_size_); } else { auto & vector = *reinterpret_cast *>(field); - if (call_new) { - new(&vector) std::vector; - } deser >> vector; } } @@ -782,10 +625,8 @@ template<> inline void deserialize_field( const rosidl_typesupport_introspection_cpp::MessageMember * member, void * field, - eprosima::fastcdr::Cdr & deser, - bool call_new) + eprosima::fastcdr::Cdr & deser) { - (void)call_new; std::wstring wstr; if (!member->is_array_) { deser >> wstr; @@ -812,10 +653,8 @@ template void deserialize_field( const rosidl_typesupport_introspection_c__MessageMember * member, void * field, - eprosima::fastcdr::Cdr & deser, - bool call_new) + eprosima::fastcdr::Cdr & deser) { - (void)call_new; if (!member->is_array_) { deser >> *static_cast(field); } else if (member->array_size_ && !member->is_upper_bound_) { @@ -833,13 +672,11 @@ template<> inline void deserialize_field( const rosidl_typesupport_introspection_c__MessageMember * member, void * field, - eprosima::fastcdr::Cdr & deser, - bool call_new) + eprosima::fastcdr::Cdr & deser) { - (void)call_new; if (!member->is_array_) { using CStringHelper = StringHelper; - CStringHelper::assign(deser, field, call_new); + CStringHelper::assign(deser, field); } else { if (member->array_size_ && !member->is_upper_bound_) { auto deser_field = static_cast(field); @@ -881,10 +718,8 @@ template<> inline void deserialize_field( const rosidl_typesupport_introspection_c__MessageMember * member, void * field, - eprosima::fastcdr::Cdr & deser, - bool call_new) + eprosima::fastcdr::Cdr & deser) { - (void)call_new; std::wstring wstr; if (!member->is_array_) { deser >> wstr; @@ -910,54 +745,11 @@ inline void deserialize_field( } } -inline size_t get_submessage_array_deserialize( - const rosidl_typesupport_introspection_cpp::MessageMember * member, - eprosima::fastcdr::Cdr & deser, - void * field, - void * & subros_message, - bool call_new, - size_t sub_members_size, - size_t max_align) -{ - (void)member; - uint32_t vsize = 0; - // Deserialize length - deser >> vsize; - auto vector = reinterpret_cast *>(field); - if (call_new) { - new(vector) std::vector; - } - void * ptr = reinterpret_cast(sub_members_size); - vector->resize(vsize * (size_t)align_(max_align, ptr)); - subros_message = reinterpret_cast(vector->data()); - return vsize; -} - -inline size_t get_submessage_array_deserialize( - const rosidl_typesupport_introspection_c__MessageMember * member, - eprosima::fastcdr::Cdr & deser, - void * field, - void * & subros_message, - bool, - size_t sub_members_size, - size_t) -{ - (void)member; - // Deserialize length - uint32_t vsize = 0; - deser >> vsize; - auto tmpsequence = static_cast(field); - rosidl_runtime_c__void__Sequence__init(tmpsequence, vsize, sub_members_size); - subros_message = reinterpret_cast(tmpsequence->data); - return vsize; -} - template bool TypeSupport::deserializeROSmessage( eprosima::fastcdr::Cdr & deser, const MembersType * members, - void * ros_message, - bool call_new) const + void * ros_message) const { assert(members); assert(ros_message); @@ -967,72 +759,78 @@ bool TypeSupport::deserializeROSmessage( void * field = static_cast(ros_message) + member->offset_; switch (member->type_id_) { case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: - deserialize_field(member, field, deser, call_new); + deserialize_field(member, field, deser); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: { - auto sub_members = (const MembersType *)member->members_->data; + auto sub_members = static_cast(member->members_->data); if (!member->is_array_) { - deserializeROSmessage(deser, sub_members, field, call_new); + deserializeROSmessage(deser, sub_members, field); } else { - void * subros_message = nullptr; size_t array_size = 0; - size_t sub_members_size = sub_members->size_of_; - size_t max_align = calculateMaxAlign(sub_members); - bool recall_new = call_new; if (member->array_size_ && !member->is_upper_bound_) { - subros_message = field; array_size = member->array_size_; } else { - array_size = get_submessage_array_deserialize( - member, deser, field, subros_message, - call_new, sub_members_size, max_align); - recall_new = true; + uint32_t num_elems = 0; + deser >> num_elems; + array_size = static_cast(num_elems); + + if (!member->resize_function) { + RMW_SET_ERROR_MSG("unexpected error: resize function is null"); + return false; + } + member->resize_function(field, 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) { - deserializeROSmessage(deser, sub_members, subros_message, recall_new); - subros_message = static_cast(subros_message) + sub_members_size; - subros_message = align_(max_align, subros_message); + deserializeROSmessage( + deser, sub_members, + get_subros_message( + member, field, index, member->array_size_, + member->is_upper_bound_)); } } } @@ -1179,7 +977,7 @@ bool TypeSupport::deserializeROSmessage( (void)impl; if (members_->member_count_ != 0) { - TypeSupport::deserializeROSmessage(deser, members_, ros_message, false); + TypeSupport::deserializeROSmessage(deser, members_, ros_message); } else { uint8_t dump = 0; deser >> dump;