diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp new file mode 100644 index 0000000..a1326e4 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp @@ -0,0 +1,195 @@ +// Copyright 2023 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERIALIZATION_HELPERS_HPP_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERIALIZATION_HELPERS_HPP_ + +#include + +#include +#include + +#include + +namespace eprosima { +namespace fastcdr { + +template +inline eprosima::fastcdr::Cdr& operator << ( + eprosima::fastcdr::Cdr& cdr, const rosidl_runtime_cpp::CDRCompatibleFixedCapacityString& str) +{ + cdr << Capacity; + cdr.serializeArray(str.c_str(), Capacity); + return cdr; +} + +template +inline eprosima::fastcdr::Cdr& operator >> ( + eprosima::fastcdr::Cdr& cdr, rosidl_runtime_cpp::CDRCompatibleFixedCapacityString& str) +{ + std::string tmp_str; + cdr >> tmp_str; + str = tmp_str; + return cdr; +} + +} // namespace fastcdr +} // namespace eprosima + +namespace rosidl_typesupport_fastrtps_cpp { + +template +inline void get_string_size( + const std::basic_string, Allocator>& str, + size_t& current_alignment) +{ + const size_t padding = 4; + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + str.size() + 1; +} + +template +inline void get_string_size( + const std::basic_string, Allocator>& str, + size_t& current_alignment) +{ + const size_t padding = 4; + const size_t wchar_size = 4; + + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + wchar_size * (str.size() + 1); +} + +template +inline void get_string_size( + const rosidl_runtime_cpp::CDRCompatibleFixedCapacityString& /* str */, + size_t& current_alignment) +{ + const size_t padding = 4; + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + Capacity; +} + +template +struct max_string_size_helper +{ +}; + +template +struct max_string_size_helper, Allocator> > +{ + static inline void max_string_size( + bool& full_bounded, + bool& is_plain, + size_t& current_alignment, + const size_t array_size, + const size_t max_size) + { + const size_t padding = 4; + + full_bounded = false; + is_plain = false; + for (size_t index = 0; index < array_size; ++index) { + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + max_size + 1; + } + } +}; + +template +struct max_string_size_helper, Allocator> > +{ + static inline void max_string_size( + bool& full_bounded, + bool& is_plain, + size_t& current_alignment, + const size_t array_size, + const size_t max_size) + { + const size_t padding = 4; + const size_t wchar_size = 4; + + full_bounded = false; + is_plain = false; + for (size_t index = 0; index < array_size; ++index) { + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + wchar_size * (max_size + 1); + } + } +}; + +template +struct max_string_size_helper > +{ + static inline void max_string_size( + bool& /*full_bounded*/, + bool& /*is_plain*/, + size_t& current_alignment, + const size_t array_size, + const size_t /*max_size*/) + { + const size_t padding = 4; + for (size_t index = 0; index < array_size; ++index) { + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + Capacity; + } + } +}; + +template +struct max_string_size_helper > : public max_string_size_helper +{}; + +template +struct max_string_size_helper > +{ + static inline void max_string_size( + bool& full_bounded, + bool& is_plain, + size_t& current_alignment, + const size_t array_size, + const size_t max_size) + { + full_bounded = false; + is_plain = false; + + max_string_size_helper::max_string_size(full_bounded, is_plain, current_alignment, array_size, max_size); + } +}; + +template +struct max_string_size_helper > +{ + static inline void max_string_size( + bool& full_bounded, + bool& is_plain, + size_t& current_alignment, + const size_t array_size, + const size_t max_size) + { + is_plain = false; + + max_string_size_helper::max_string_size(full_bounded, is_plain, current_alignment, array_size, max_size); + } +}; + +} // namespace rosidl_typesupport_fastrtps_cpp + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERIALIZATION_HELPERS_HPP_ diff --git a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em index a02d04f..203e6e2 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -1,6 +1,7 @@ @# Included from rosidl_typesupport_fastrtps_cpp/resource/idl__type_support.cpp.em @{ from rosidl_generator_c import idl_structure_type_to_c_typename +from rosidl_generator_cpp import msg_type_to_cpp from rosidl_generator_type_description import GET_DESCRIPTION_FUNC from rosidl_generator_type_description import GET_HASH_FUNC from rosidl_generator_type_description import GET_SOURCES_FUNC @@ -21,6 +22,7 @@ header_files = [ 'rosidl_typesupport_fastrtps_cpp/identifier.hpp', 'rosidl_typesupport_fastrtps_cpp/message_type_support.h', 'rosidl_typesupport_fastrtps_cpp/message_type_support_decl.hpp', + 'rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp', 'rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp', 'fastcdr/Cdr.h', ] @@ -298,9 +300,7 @@ get_serialized_size( size_t initial_alignment = current_alignment; const size_t padding = 4; - const size_t wchar_size = 4; (void)padding; - (void)wchar_size; @[for member in message.structure.members]@ // Member: @(member.name) @@ -321,12 +321,8 @@ get_serialized_size( @[ end if]@ @[ if isinstance(member.type.value_type, AbstractGenericString)]@ for (size_t index = 0; index < array_size; ++index) { - current_alignment += padding + - eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + -@[ if isinstance(member.type.value_type, AbstractWString)]@ - wchar_size * -@[ end if]@ - (ros_message.@(member.name)[index].size() + 1); + rosidl_typesupport_fastrtps_cpp::get_string_size( + ros_message.@(member.name)[index], current_alignment); } @[ elif isinstance(member.type.value_type, BasicType)]@ size_t item_size = sizeof(ros_message.@(member.name)[0]); @@ -342,12 +338,8 @@ get_serialized_size( } @[ else]@ @[ if isinstance(member.type, AbstractGenericString)]@ - current_alignment += padding + - eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + -@[ if isinstance(member.type, AbstractWString)]@ - wchar_size * -@[ end if]@ - (ros_message.@(member.name).size() + 1); + rosidl_typesupport_fastrtps_cpp::get_string_size( + ros_message.@(member.name), current_alignment); @[ elif isinstance(member.type, BasicType)]@ { size_t item_size = sizeof(ros_message.@(member.name)); @@ -375,9 +367,7 @@ max_serialized_size_@(message.structure.namespaced_type.name)( size_t initial_alignment = current_alignment; const size_t padding = 4; - const size_t wchar_size = 4; (void)padding; - (void)wchar_size; full_bounded = true; is_plain = true; @@ -410,22 +400,11 @@ if isinstance(type_, AbstractNestedType): type_ = type_.value_type }@ @[ if isinstance(type_, AbstractGenericString)]@ - full_bounded = false; - is_plain = false; - for (size_t index = 0; index < array_size; ++index) { - current_alignment += padding + - eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + -@[ if type_.has_maximum_size()]@ -@[ if isinstance(type_, AbstractWString)]@ - wchar_size * -@[ end if]@ - @(type_.maximum_size) + -@[ end if]@ -@[ if isinstance(type_, AbstractWString)]@ - wchar_size * -@[ end if]@ - 1; - } + rosidl_typesupport_fastrtps_cpp::max_string_size_helper< + decltype(@('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name] + [member.name])))> + ::max_string_size( + full_bounded, is_plain, current_alignment, array_size, + @(type_.has_maximum_size() ? type_.maximum_size : 0)); @[ elif isinstance(type_, BasicType)]@ @[ if type_.typename in ('boolean', 'octet', 'char', 'uint8', 'int8')]@ current_alignment += array_size * sizeof(uint8_t);