diff --git a/rosidl_typesupport_fastrtps_c/resource/msg__type_support_c.cpp.em b/rosidl_typesupport_fastrtps_c/resource/msg__type_support_c.cpp.em index 4a518d9..5a9f0c0 100644 --- a/rosidl_typesupport_fastrtps_c/resource/msg__type_support_c.cpp.em +++ b/rosidl_typesupport_fastrtps_c/resource/msg__type_support_c.cpp.em @@ -19,8 +19,17 @@ from rosidl_parser.definition import Array from rosidl_parser.definition import BasicType from rosidl_parser.definition import BoundedSequence from rosidl_parser.definition import NamespacedType +from rosidl_parser.definition import UnboundedSequence from rosidl_pycommon import convert_camel_case_to_lower_case_underscore +# Detect if message has Buffer fields (only uint8[] UnboundedSequence becomes Buffer) +has_buffer_fields = False +for member in message.structure.members: + if isinstance(member.type, UnboundedSequence): + if isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'uint8': + has_buffer_fields = True + break + include_parts = [package_name] + list(interface_path.parents[0].parts) + [ 'detail', convert_camel_case_to_lower_case_underscore(interface_path.stem)] include_base = '/'.join(include_parts) @@ -41,6 +50,8 @@ header_files = [ include_base + '__functions.h', 'fastcdr/Cdr.h', ] +if has_buffer_fields: + header_files.append('rosidl_typesupport_fastrtps_cpp/buffer_serialization.hpp') }@ @[for header_file in header_files]@ @[ if header_file in include_directives]@ @@ -55,6 +66,7 @@ header_files = [ #include "@(header_file)" @[ end if]@ @[end for]@ +@# Buffer-backed uint8[] fields use the is_rosidl_buffer flag on the sequence struct. #ifndef _WIN32 # pragma GCC diagnostic push @@ -218,6 +230,7 @@ def generate_member_for_cdr_serialize(member, suffix): from rosidl_parser.definition import BasicType from rosidl_parser.definition import BoundedSequence from rosidl_parser.definition import NamespacedType + from rosidl_parser.definition import UnboundedSequence strlist = [] strlist.append('// Field name: %s' % (member.name)) strlist.append('{') @@ -226,7 +239,30 @@ def generate_member_for_cdr_serialize(member, suffix): if isinstance(type_, AbstractNestedType): type_ = type_.value_type - if isinstance(member.type, AbstractNestedType): + if ( + suffix == '' and + isinstance(member.type, UnboundedSequence) and + isinstance(member.type.value_type, BasicType) and + member.type.value_type.typename == 'uint8' + ): + strlist.append(' // Regular path CPU fallback for rosidl_buffer-backed uint8[]') + strlist.append(' if (ros_message->%s.is_rosidl_buffer) {' % (member.name)) + strlist.append( + ' auto * buffer = reinterpret_cast *>(ros_message->%s.data);' % + (member.name)) + strlist.append(' if (buffer == nullptr) {') + strlist.append(' fprintf(stderr, "null rosidl_buffer pointer for field \'%s\'\\n");' % (member.name)) + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' const std::vector vec = buffer->to_vector();') + strlist.append(' cdr << vec;') + strlist.append(' } else {') + strlist.append(' size_t size = ros_message->%s.size;' % (member.name)) + strlist.append(' auto array_ptr = ros_message->%s.data;' % (member.name)) + strlist.append(' cdr << static_cast(size);') + strlist.append(' cdr.serialize_array(array_ptr, size);') + strlist.append(' }') + elif isinstance(member.type, AbstractNestedType): if isinstance(member.type, Array): strlist.append(' size_t size = %d;' % (member.type.size)) strlist.append(' auto array_ptr = ros_message->%s;' % (member.name)) @@ -302,6 +338,188 @@ def generate_member_for_cdr_serialize(member, suffix): strlist.append('}') return strlist + + +# Generates deserialization code for a single member as a list of strings. +# Used by both the regular and _with_endpoint deserializers. +def generate_member_for_cdr_deserialize(member): + from rosidl_parser.definition import AbstractGenericString + from rosidl_parser.definition import AbstractNestedType + from rosidl_parser.definition import AbstractSequence + from rosidl_parser.definition import AbstractString + from rosidl_parser.definition import AbstractWString + from rosidl_parser.definition import Array + from rosidl_parser.definition import BasicType + from rosidl_parser.definition import BoundedSequence + from rosidl_parser.definition import NamespacedType + from rosidl_parser.definition import UnboundedSequence + strlist = [] + strlist.append('// Field name: %s' % (member.name)) + strlist.append('{') + + type_ = member.type + if isinstance(type_, AbstractNestedType): + type_ = type_.value_type + + if ( + isinstance(member.type, UnboundedSequence) and + isinstance(member.type.value_type, BasicType) and + member.type.value_type.typename == 'uint8' + ): + strlist.append(' // Regular path CPU fallback for rosidl_buffer-backed uint8[]') + strlist.append(' if (ros_message->%s.is_rosidl_buffer) {' % member.name) + strlist.append( + ' auto * old_buffer = reinterpret_cast *>(ros_message->%s.data);' % + member.name) + strlist.append(' delete old_buffer;') + strlist.append(' ros_message->%s.data = nullptr;' % member.name) + strlist.append(' ros_message->%s.size = 0;' % member.name) + strlist.append(' ros_message->%s.capacity = 0;' % member.name) + strlist.append(' ros_message->%s.is_rosidl_buffer = false;' % member.name) + strlist.append(' }') + strlist.append(' std::vector vec;') + strlist.append(' cdr >> vec;') + strlist.append(' size_t size = vec.size();') + strlist.append(' if (ros_message->%s.data) {' % member.name) + strlist.append(' rosidl_runtime_c__uint8__Sequence__fini(&ros_message->%s);' % member.name) + strlist.append(' }') + strlist.append(' if (!rosidl_runtime_c__uint8__Sequence__init(&ros_message->%s, size)) {' % member.name) + strlist.append(' fprintf(stderr, "failed to create array for field \'%s\'");' % member.name) + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' auto array_ptr = ros_message->%s.data;' % member.name) + strlist.append(' for (size_t i = 0; i < size; ++i) {') + strlist.append(' array_ptr[i] = vec[i];') + strlist.append(' }') + strlist.append(' ros_message->%s.is_rosidl_buffer = false;' % member.name) + elif isinstance(member.type, AbstractNestedType): + if isinstance(member.type, Array): + strlist.append(' size_t size = %d;' % (member.type.size)) + strlist.append(' auto array_ptr = ros_message->%s;' % (member.name)) + else: + # Compute init/fini function names + if isinstance(member.type.value_type, AbstractString): + array_init = 'rosidl_runtime_c__String__Sequence__init' + array_fini = 'rosidl_runtime_c__String__Sequence__fini' + elif isinstance(member.type.value_type, AbstractWString): + array_init = 'rosidl_runtime_c__U16String__Sequence__init' + array_fini = 'rosidl_runtime_c__U16String__Sequence__fini' + elif isinstance(member.type.value_type, BasicType): + bt = member.type.value_type.typename.replace(' ', '_') + array_init = 'rosidl_runtime_c__%s__Sequence__init' % bt + array_fini = 'rosidl_runtime_c__%s__Sequence__fini' % bt + else: + array_init = '__'.join(type_.namespaced_name()) + '__Sequence__init' + array_fini = '__'.join(type_.namespaced_name()) + '__Sequence__fini' + strlist.append(' uint32_t cdrSize;') + strlist.append(' cdr >> cdrSize;') + strlist.append(' size_t size = static_cast(cdrSize);') + strlist.append('') + strlist.append(' // Check there are at least \'size\' remaining bytes in the CDR stream before resizing') + strlist.append(' auto old_state = cdr.get_state();') + strlist.append(' bool correct_size = cdr.jump(size);') + strlist.append(' cdr.set_state(old_state);') + strlist.append(' if (!correct_size) {') + strlist.append(' fprintf(stderr, "sequence size exceeds remaining buffer\\n");') + strlist.append(' return false;') + strlist.append(' }') + strlist.append('') + strlist.append(' if (ros_message->%s.data) {' % member.name) + strlist.append(' %s(&ros_message->%s);' % (array_fini, member.name)) + strlist.append(' }') + strlist.append(' if (!%s(&ros_message->%s, size)) {' % (array_init, member.name)) + strlist.append(' fprintf(stderr, "failed to create array for field \'%s\'");' % member.name) + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' auto array_ptr = ros_message->%s.data;' % member.name) + + # Element deserialization + if isinstance(member.type.value_type, AbstractString): + strlist.append(' for (size_t i = 0; i < size; ++i) {') + strlist.append(' std::string tmp;') + strlist.append(' cdr >> tmp;') + strlist.append(' auto & ros_i = array_ptr[i];') + strlist.append(' if (!ros_i.data) {') + strlist.append(' rosidl_runtime_c__String__init(&ros_i);') + strlist.append(' }') + strlist.append(' bool succeeded = rosidl_runtime_c__String__assign(&ros_i, tmp.c_str());') + strlist.append(' if (!succeeded) {') + strlist.append(' fprintf(stderr, "failed to assign string into field \'%s\'\\n");' % member.name) + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' }') + elif isinstance(member.type.value_type, AbstractWString): + strlist.append(' std::wstring wstr;') + strlist.append(' for (size_t i = 0; i < size; ++i) {') + strlist.append(' auto & ros_i = array_ptr[i];') + strlist.append(' if (!ros_i.data) {') + strlist.append(' rosidl_runtime_c__U16String__init(&ros_i);') + strlist.append(' }') + strlist.append(' bool succeeded = rosidl_typesupport_fastrtps_c::cdr_deserialize(cdr, ros_i);') + strlist.append(' if (!succeeded) {') + strlist.append(' fprintf(stderr, "failed to create wstring from u16string\\n");') + strlist.append(' rosidl_runtime_c__U16String__fini(&ros_i);') + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' }') + elif isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'boolean': + strlist.append(' for (size_t i = 0; i < size; ++i) {') + strlist.append(' uint8_t tmp;') + strlist.append(' cdr >> tmp;') + strlist.append(' array_ptr[i] = tmp ? true : false;') + strlist.append(' }') + elif isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'wchar': + strlist.append(' for (size_t i = 0; i < size; ++i) {') + strlist.append(' wchar_t tmp;') + strlist.append(' cdr >> tmp;') + strlist.append(' array_ptr[i] = static_cast(tmp);') + strlist.append(' }') + elif isinstance(member.type.value_type, BasicType): + strlist.append(' cdr.deserialize_array(array_ptr, size);') + else: + strlist.append(' for (size_t i = 0; i < size; ++i) {') + strlist.append(' cdr_deserialize_%s(cdr, &array_ptr[i]);' % '__'.join(member.type.value_type.namespaced_name())) + strlist.append(' }') + + elif isinstance(member.type, AbstractString): + strlist.append(' std::string tmp;') + strlist.append(' cdr >> tmp;') + strlist.append(' if (!ros_message->%s.data) {' % member.name) + strlist.append(' rosidl_runtime_c__String__init(&ros_message->%s);' % member.name) + strlist.append(' }') + strlist.append(' bool succeeded = rosidl_runtime_c__String__assign(') + strlist.append(' &ros_message->%s,' % member.name) + strlist.append(' tmp.c_str());') + strlist.append(' if (!succeeded) {') + strlist.append(' fprintf(stderr, "failed to assign string into field \'%s\'\\n");' % member.name) + strlist.append(' return false;') + strlist.append(' }') + elif isinstance(member.type, AbstractWString): + strlist.append(' if (!ros_message->%s.data) {' % member.name) + strlist.append(' rosidl_runtime_c__U16String__init(&ros_message->%s);' % member.name) + strlist.append(' }') + strlist.append(' bool succeeded = rosidl_typesupport_fastrtps_c::cdr_deserialize(cdr, ros_message->%s);' % member.name) + strlist.append(' if (!succeeded) {') + strlist.append(' fprintf(stderr, "failed to create wstring from u16string\\n");') + strlist.append(' rosidl_runtime_c__U16String__fini(&ros_message->%s);' % member.name) + strlist.append(' return false;') + strlist.append(' }') + elif isinstance(member.type, BasicType) and member.type.typename == 'boolean': + strlist.append(' uint8_t tmp;') + strlist.append(' cdr >> tmp;') + strlist.append(' ros_message->%s = tmp ? true : false;' % member.name) + elif isinstance(member.type, BasicType) and member.type.typename == 'wchar': + strlist.append(' wchar_t tmp;') + strlist.append(' cdr >> tmp;') + strlist.append(' ros_message->%s = static_cast(tmp);' % member.name) + elif isinstance(member.type, BasicType): + strlist.append(' cdr >> ros_message->%s;' % member.name) + else: + strlist.append(' cdr_deserialize_%s(cdr, &ros_message->%s);' % ('__'.join(member.type.namespaced_name()), member.name)) + + strlist.append('}') + return strlist + }@ ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) @@ -324,142 +542,13 @@ bool cdr_deserialize_@('__'.join([package_name] + list(interface_path.parents[0] @('__'.join(message.structure.namespaced_type.namespaced_name())) * ros_message) { @[for member in message.structure.members]@ - // Field name: @(member.name) - { -@{ -type_ = member.type -if isinstance(type_, AbstractNestedType): - type_ = type_.value_type -}@ -@[ if isinstance(member.type, AbstractNestedType)]@ -@[ if isinstance(member.type, Array)]@ - size_t size = @(member.type.size); - auto array_ptr = ros_message->@(member.name); +@[ for line in generate_member_for_cdr_deserialize(member)]@ +@[ if line]@ + @(line) @[ else]@ -@{ -if isinstance(member.type.value_type, AbstractString): - array_init = 'rosidl_runtime_c__String__Sequence__init' - array_fini = 'rosidl_runtime_c__String__Sequence__fini' -elif isinstance(member.type.value_type, AbstractWString): - array_init = 'rosidl_runtime_c__U16String__Sequence__init' - array_fini = 'rosidl_runtime_c__U16String__Sequence__fini' -elif isinstance(member.type.value_type, BasicType): - type_ = member.type.value_type.typename - type_ = type_.replace(' ', '_') - array_init = 'rosidl_runtime_c__{type_}__Sequence__init'.format(**locals()) - array_fini = 'rosidl_runtime_c__{type_}__Sequence__fini'.format(**locals()) -else: - array_init = '__'.join(type_.namespaced_name()) + '__Sequence__init' - array_fini = '__'.join(type_.namespaced_name()) + '__Sequence__fini' -}@ - uint32_t cdrSize; - cdr >> cdrSize; - size_t size = static_cast(cdrSize); - - // Check there are at least 'size' remaining bytes in the CDR stream before resizing - auto old_state = cdr.get_state(); - bool correct_size = cdr.jump(size); - cdr.set_state(old_state); - if (!correct_size) { - fprintf(stderr, "sequence size exceeds remaining buffer\n"); - return false; - } - if (ros_message->@(member.name).data) { - @(array_fini)(&ros_message->@(member.name)); - } - if (!@(array_init)(&ros_message->@(member.name), size)) { - fprintf(stderr, "failed to create array for field '@(member.name)'"); - return false; - } - auto array_ptr = ros_message->@(member.name).data; @[ end if]@ -@[ if isinstance(member.type.value_type, AbstractString)]@ - for (size_t i = 0; i < size; ++i) { - std::string tmp; - cdr >> tmp; - auto & ros_i = array_ptr[i]; - if (!ros_i.data) { - rosidl_runtime_c__String__init(&ros_i); - } - bool succeeded = rosidl_runtime_c__String__assign( - &ros_i, - tmp.c_str()); - if (!succeeded) { - fprintf(stderr, "failed to assign string into field '@(member.name)'\n"); - return false; - } - } -@[ elif isinstance(member.type.value_type, AbstractWString)]@ - std::wstring wstr; - for (size_t i = 0; i < size; ++i) { - auto & ros_i = array_ptr[i]; - if (!ros_i.data) { - rosidl_runtime_c__U16String__init(&ros_i); - } - bool succeeded = rosidl_typesupport_fastrtps_c::cdr_deserialize(cdr, ros_i); - if (!succeeded) { - fprintf(stderr, "failed to create wstring from u16string\n"); - rosidl_runtime_c__U16String__fini(&ros_i); - return false; - } - } -@[ elif isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'boolean']@ - for (size_t i = 0; i < size; ++i) { - uint8_t tmp; - cdr >> tmp; - array_ptr[i] = tmp ? true : false; - } -@[ elif isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'wchar']@ - for (size_t i = 0; i < size; ++i) { - wchar_t tmp; - cdr >> tmp; - array_ptr[i] = static_cast(tmp); - } -@[ elif isinstance(member.type.value_type, BasicType)]@ - cdr.deserialize_array(array_ptr, size); -@[ else]@ - for (size_t i = 0; i < size; ++i) { - cdr_deserialize_@('__'.join(member.type.value_type.namespaced_name()))(cdr, &array_ptr[i]); - } -@[ end if]@ -@[ elif isinstance(member.type, AbstractString)]@ - std::string tmp; - cdr >> tmp; - if (!ros_message->@(member.name).data) { - rosidl_runtime_c__String__init(&ros_message->@(member.name)); - } - bool succeeded = rosidl_runtime_c__String__assign( - &ros_message->@(member.name), - tmp.c_str()); - if (!succeeded) { - fprintf(stderr, "failed to assign string into field '@(member.name)'\n"); - return false; - } -@[ elif isinstance(member.type, AbstractWString)]@ - if (!ros_message->@(member.name).data) { - rosidl_runtime_c__U16String__init(&ros_message->@(member.name)); - } - bool succeeded = rosidl_typesupport_fastrtps_c::cdr_deserialize(cdr, ros_message->@(member.name)); - if (!succeeded) { - fprintf(stderr, "failed to create wstring from u16string\n"); - rosidl_runtime_c__U16String__fini(&ros_message->@(member.name)); - return false; - } -@[ elif isinstance(member.type, BasicType) and member.type.typename == 'boolean']@ - uint8_t tmp; - cdr >> tmp; - ros_message->@(member.name) = tmp ? true : false; -@[ elif isinstance(member.type, BasicType) and member.type.typename == 'wchar']@ - wchar_t tmp; - cdr >> tmp; - ros_message->@(member.name) = static_cast(tmp); -@[ elif isinstance(member.type, BasicType)]@ - cdr >> ros_message->@(member.name); -@[ else]@ - cdr_deserialize_@('__'.join(member.type.namespaced_name()))(cdr, &ros_message->@(member.name)); -@[ end if]@ - } +@[ end for]@ @[end for]@ return true; @@ -483,8 +572,33 @@ def generate_member_for_get_serialized_size(member, suffix): from rosidl_parser.definition import BasicType from rosidl_parser.definition import BoundedSequence from rosidl_parser.definition import NamespacedType + from rosidl_parser.definition import UnboundedSequence strlist = [] strlist.append('// Field name: %s' % (member.name)) + + # For uint8[] UnboundedSequence (Buffer), handle is_rosidl_buffer + # where ros_message->field.size may be 0 but the actual Buffer has data. + if ( + suffix == '' and + isinstance(member.type, UnboundedSequence) and + isinstance(member.type.value_type, BasicType) and + member.type.value_type.typename == 'uint8' + ): + strlist.append('{') + strlist.append(' size_t array_size;') + strlist.append(' if (ros_message->%s.is_rosidl_buffer) {' % member.name) + strlist.append(' auto * buffer = reinterpret_cast *>(ros_message->%s.data);' % member.name) + strlist.append(' array_size = (buffer != nullptr) ? buffer->size() : 0;') + strlist.append(' } else {') + strlist.append(' array_size = ros_message->%s.size;' % member.name) + strlist.append(' }') + strlist.append(' current_alignment += padding +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, padding);') + strlist.append(' current_alignment += array_size * sizeof(uint8_t) +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint8_t));') + strlist.append('}') + return strlist + if isinstance(member.type, AbstractNestedType): strlist.append('{') if isinstance(member.type, Array): @@ -885,6 +999,119 @@ static size_t _@(message.structure.namespaced_type.name)__max_serialized_size(ch } @ +@[if has_buffer_fields]@ +// Endpoint-aware serialization for C messages with Buffer fields. +// Uses the same per-field serialization as the regular path, but for uint8[] fields +// checks the is_rosidl_buffer flag to detect rosidl::Buffer*. +static bool _@(message.structure.namespaced_type.name)__cdr_serialize_with_endpoint( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr, + const rmw_topic_endpoint_info_t & endpoint_info, + const rosidl_typesupport_fastrtps_cpp::BufferSerializationContext & serialization_context) +{ + if (!untyped_ros_message) { + fprintf(stderr, "ros message handle is null\n"); + return false; + } + const @('__'.join(message.structure.namespaced_type.namespaced_name())) * ros_message = + static_cast(untyped_ros_message); + (void)endpoint_info; +@[ for member in message.structure.members]@ +@[ if isinstance(member.type, UnboundedSequence) and isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'uint8']@ + // Field name: @(member.name) (buffer-aware) + { + if (ros_message->@(member.name).is_rosidl_buffer) { + auto * buffer = reinterpret_cast *>( + ros_message->@(member.name).data); + rosidl_typesupport_fastrtps_cpp::serialize_buffer_with_endpoint( + cdr, *buffer, endpoint_info, serialization_context); + } else { + // Normal sequence: serialize as legacy uint8[] wire format + // (uint32 size + raw bytes) for strict compatibility. + size_t size = ros_message->@(member.name).size; + cdr << static_cast(size); + if (size > 0) { + cdr.serialize_array(ros_message->@(member.name).data, size); + } + } + } +@[ else]@ + // Field name: @(member.name) +@[ for line in generate_member_for_cdr_serialize(member, '')]@ + @(line) +@[ end for]@ +@[ end if]@ + +@[ end for]@ + return true; +} + +// Endpoint-aware deserialization for C messages with Buffer fields. +// For vendor-backed buffer data, creates a heap-allocated rosidl::Buffer +// and sets is_rosidl_buffer on the C sequence struct. +static bool _@(message.structure.namespaced_type.name)__cdr_deserialize_with_endpoint( + eprosima::fastcdr::Cdr & cdr, + void * untyped_ros_message, + const rmw_topic_endpoint_info_t & endpoint_info, + const rosidl_typesupport_fastrtps_cpp::BufferSerializationContext & serialization_context) +{ + if (!untyped_ros_message) { + fprintf(stderr, "ros message handle is null\n"); + return false; + } + @('__'.join(message.structure.namespaced_type.namespaced_name())) * ros_message = + static_cast<@('__'.join(message.structure.namespaced_type.namespaced_name())) *>(untyped_ros_message); + (void)endpoint_info; +@[ for member in message.structure.members]@ +@[ if isinstance(member.type, UnboundedSequence) and isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'uint8']@ + // Field name: @(member.name) (buffer-aware) + { + // Deserialize into a temporary Buffer, then decide: is_rosidl_buffer or copy + auto * buffer = new rosidl::Buffer(); + try { + rosidl_typesupport_fastrtps_cpp::deserialize_buffer_with_endpoint( + cdr, *buffer, endpoint_info, serialization_context); + } catch (const std::exception & e) { + delete buffer; + fprintf(stderr, "Failed to deserialize buffer field '@(member.name)': %s\n", e.what()); + return false; + } + + if (buffer->get_backend_type() != "cpu") { + ros_message->@(member.name).data = reinterpret_cast(buffer); + ros_message->@(member.name).size = buffer->size(); + ros_message->@(member.name).capacity = 0; + ros_message->@(member.name).is_rosidl_buffer = true; + ros_message->@(member.name).owns_rosidl_buffer = true; + } else { + // CPU backend: copy into normal sequence (backward compatible) + size_t buf_size = buffer->size(); + if (!rosidl_runtime_c__uint8__Sequence__init(&ros_message->@(member.name), buf_size)) { + delete buffer; + fprintf(stderr, "Failed to init uint8 sequence for '@(member.name)'\n"); + return false; + } + if (buf_size > 0) { + memcpy(ros_message->@(member.name).data, buffer->data(), buf_size); + } + delete buffer; + } + } +@[ else]@ + // Field name: @(member.name) +@[ for line in generate_member_for_cdr_deserialize(member)]@ +@[ if line]@ + @(line) +@[ else]@ + +@[ end if]@ +@[ end for]@ +@[ end if]@ + +@[ end for]@ + return true; +} // NOLINT(readability/fn_size) +@[end if]@ @# // Collect the callback functions and provide a function to get the type support struct. static message_type_support_callbacks_t __callbacks_@(message.structure.namespaced_type.name) = { @@ -895,8 +1122,16 @@ static message_type_support_callbacks_t __callbacks_@(message.structure.namespac _@(message.structure.namespaced_type.name)__get_serialized_size, _@(message.structure.namespaced_type.name)__max_serialized_size, @[ if message.structure.has_any_member_with_annotation('key') ]@ - &__key_callbacks_@(message.structure.namespaced_type.name) + &__key_callbacks_@(message.structure.namespaced_type.name), +@[ else]@ + nullptr, +@[ end if]@ + @('true' if has_buffer_fields else 'false'), +@[ if has_buffer_fields]@ + _@(message.structure.namespaced_type.name)__cdr_serialize_with_endpoint, + _@(message.structure.namespaced_type.name)__cdr_deserialize_with_endpoint @[ else]@ + nullptr, nullptr @[ end if]@ }; diff --git a/rosidl_typesupport_fastrtps_cpp/CMakeLists.txt b/rosidl_typesupport_fastrtps_cpp/CMakeLists.txt index c5cea2f..783cd74 100644 --- a/rosidl_typesupport_fastrtps_cpp/CMakeLists.txt +++ b/rosidl_typesupport_fastrtps_cpp/CMakeLists.txt @@ -28,10 +28,14 @@ find_package(fastcdr 2 REQUIRED CONFIG) find_package(ament_cmake_python REQUIRED) find_package(rmw REQUIRED) +find_package(rosidl_buffer_backend REQUIRED) find_package(rosidl_runtime_c REQUIRED) +find_package(rosidl_runtime_cpp REQUIRED) ament_export_dependencies(rmw) +ament_export_dependencies(rosidl_buffer_backend) ament_export_dependencies(rosidl_runtime_c) +ament_export_dependencies(rosidl_runtime_cpp) ament_export_dependencies(fastcdr) ament_python_install_package(${PROJECT_NAME}) @@ -45,7 +49,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$") target_link_libraries(${PROJECT_NAME} PUBLIC fastcdr + rosidl_buffer_backend::rosidl_buffer_backend rosidl_runtime_c::rosidl_runtime_c + rosidl_runtime_cpp::rosidl_runtime_cpp rmw::rmw) # Export old-style CMake variables @@ -83,6 +89,14 @@ if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) ament_add_pytest_test(test_cli_extension test/test_cli_extension.py) + + ament_add_gtest(test_buffer_wire_compat + test/test_buffer_wire_compat.cpp) + if(TARGET test_buffer_wire_compat) + target_link_libraries(test_buffer_wire_compat + ${PROJECT_NAME} + fastcdr) + endif() endif() ament_package( diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/buffer_serialization.hpp b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/buffer_serialization.hpp new file mode 100644 index 0000000..01a4cd6 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/buffer_serialization.hpp @@ -0,0 +1,327 @@ +// Copyright 2026 Open Source Robotics Foundation, Inc. +// +// 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__BUFFER_SERIALIZATION_HPP_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_CPP__BUFFER_SERIALIZATION_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rosidl_buffer/buffer.hpp" +#include "rosidl_buffer_backend/buffer_descriptor_ops.hpp" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support_decl.hpp" +#include "rosidl_typesupport_fastrtps_cpp/visibility_control.h" +#include "fastcdr/Cdr.h" +#include "rmw/topic_endpoint_info.h" +#include "rcutils/logging_macros.h" + +namespace rosidl_typesupport_fastrtps_cpp +{ + +/// Forward declaration — BufferDescriptorSerializers and BufferSerializationContext are +/// intentionally mutually dependent: +/// +/// BufferDescriptorSerializers references BufferSerializationContext (by const-ref +/// in its std::function signatures) +/// BufferSerializationContext contains BufferDescriptorSerializers (by value) +/// +/// This circularity exists because descriptor messages themselves may contain Buffer +/// fields (e.g. uint8[] data in a descriptor .msg). When such a field is backed by a +/// non-CPU backend, the generated cdr_serialize_with_endpoint for the descriptor message +/// calls serialize_buffer_with_endpoint, which needs the full context to look up the +/// inner backend's ops and serializers. +struct BufferSerializationContext; + +/// FastCDR-specific descriptor serialization functions (technology-specific). +struct BufferDescriptorSerializers +{ + std::function &, + const rmw_topic_endpoint_info_t &, const BufferSerializationContext &)> serialize; + std::function(eprosima::fastcdr::Cdr &, + const rmw_topic_endpoint_info_t &, const BufferSerializationContext &)> deserialize; +}; + +/// RMW-owned descriptor context passed through endpoint-aware callbacks. +struct BufferSerializationContext +{ + std::unordered_map descriptor_ops; + std::unordered_map descriptor_serializers; +}; + +/// Marker for descriptor-backed Buffer payloads. +/// CPU/legacy vector path: first uint32 is the sequence length (any value != marker). +/// Descriptor path: first uint32 == kBufferDescriptorMarker, followed by backend_type +/// string and the serialized descriptor. +inline constexpr uint32_t kBufferDescriptorMarker = 0xFFFFFFFFu; + +/// Get serialized size of Buffer - for use by generated type support code +template +inline size_t get_buffer_serialized_size( + const rosidl::Buffer & buffer, + size_t current_alignment) +{ + size_t initial_alignment = current_alignment; + const size_t padding = 4; + + const std::string backend_type = buffer.get_backend_type(); + + if (backend_type == "cpu") { + // CPU path is wire-compatible with std::vector: + // uint32 length + element bytes. + size_t array_size = buffer.size(); + + // Align to 4-byte boundary for the length field + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + // Add 4 bytes for the array length + current_alignment += padding; + + // Add array elements + if (array_size > 0) { + size_t item_size = sizeof(T); + // Elements might need alignment + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); + current_alignment += array_size * item_size; + } + } else { + // Descriptor marker prefix. + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + + // backend_type string + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + backend_type.size() + 1; // +1 for null terminator + + // Vendor backends: account for descriptor payload + // Conservative estimate: buffer data size + overhead for metadata fields + size_t buffer_data_size = buffer.size() * sizeof(T); + size_t metadata_overhead = 256; + current_alignment += buffer_data_size + metadata_overhead; + } + + return current_alignment - initial_alignment; +} + +/// Serialize Buffer with endpoint awareness. +/// Calls endpoint-specific descriptor creation for optimization. +/// If the backend returns nullptr from create_descriptor_with_endpoint(), the buffer +/// is serialized as std::vector (CPU fallback) for legacy wire compatibility. +template +inline void serialize_buffer_with_endpoint( + eprosima::fastcdr::Cdr & cdr, + const rosidl::Buffer & buffer, + const rmw_topic_endpoint_info_t & endpoint_info, + const BufferSerializationContext & serialization_context) +{ + const std::string backend_type = buffer.get_backend_type(); + + RCUTILS_LOG_INFO_NAMED("serialize_buffer_with_endpoint", + ("Serializing buffer (backend: " + backend_type + ")").c_str()); + + if (backend_type == "cpu") { + RCUTILS_LOG_INFO_NAMED("serialize_buffer_with_endpoint", "Serializing buffer as std::vector"); + std::vector vec = buffer.to_vector(); + cdr << vec; + return; + } + + const auto * impl = buffer.get_impl(); + if (!impl) { + throw std::runtime_error("Buffer implementation is null"); + } + + auto ops_it = serialization_context.descriptor_ops.find(backend_type); + auto ser_it = serialization_context.descriptor_serializers.find(backend_type); + if (ops_it == serialization_context.descriptor_ops.end() || + ser_it == serialization_context.descriptor_serializers.end()) + { + RCUTILS_LOG_WARN_NAMED( + "serialize_buffer_with_endpoint", + "Backend '%s' not available (shutdown?), falling back to CPU wire format", + backend_type.c_str()); + std::vector vec = buffer.to_vector(); + cdr << vec; + return; + } + + auto * non_const_impl = const_cast *>(impl); + std::shared_ptr impl_shared(static_cast(non_const_impl), [](void *){}); + + auto descriptor = ops_it->second.create_descriptor_with_endpoint(impl_shared, endpoint_info); + + // nullptr means the backend cannot handle this endpoint — fall back to CPU wire format. + if (!descriptor) { + RCUTILS_LOG_INFO_NAMED( + "serialize_buffer_with_endpoint", "Backend returned null descriptor, falling back to CPU"); + std::vector vec = buffer.to_vector(); + cdr << vec; + return; + } + + // Descriptor-backed payload marker in first uint32. + cdr << static_cast(kBufferDescriptorMarker); + cdr << backend_type; + + RCUTILS_LOG_INFO_NAMED("serialize_buffer_with_endpoint", + ("Serializing descriptor for backend: " + backend_type).c_str()); + + ser_it->second.serialize(cdr, descriptor, endpoint_info, serialization_context); +} + +/// Deserialize Buffer with endpoint awareness. +/// Returns true on success, false if deserialization could not be completed +/// (e.g. backend unavailable after shutdown). +template +inline bool deserialize_buffer_with_endpoint( + eprosima::fastcdr::Cdr & cdr, + rosidl::Buffer & buffer, + const rmw_topic_endpoint_info_t & endpoint_info, + const BufferSerializationContext & serialization_context) +{ + RCUTILS_LOG_INFO_NAMED("deserialize_buffer_with_endpoint", "Starting buffer deserialization"); + + // Peek first uint32 to disambiguate legacy vector bytes vs descriptor payload. + auto original_state = cdr.get_state(); + uint32_t first_word = 0u; + cdr >> first_word; + cdr.set_state(original_state); + + // Legacy/vector path: first word is a sequence length (any value != marker). + if (first_word != kBufferDescriptorMarker) { + RCUTILS_LOG_INFO_NAMED( + "deserialize_buffer_with_endpoint", "Legacy vector path: deserializing std::vector"); + std::vector vec; + cdr >> vec; + + buffer.resize(vec.size()); + for (size_t i = 0; i < vec.size(); ++i) { + buffer[i] = vec[i]; + } + return true; + } + + // Descriptor path: consume the marker. + cdr >> first_word; + + std::string backend_type; + cdr >> backend_type; + RCUTILS_LOG_INFO_NAMED("deserialize_buffer_with_endpoint", + (backend_type + " backend: deserializing descriptor").c_str()); + + auto ops_it = serialization_context.descriptor_ops.find(backend_type); + auto ser_it = serialization_context.descriptor_serializers.find(backend_type); + if (ops_it == serialization_context.descriptor_ops.end() || + ser_it == serialization_context.descriptor_serializers.end()) + { + RCUTILS_LOG_ERROR_NAMED( + "deserialize_buffer_with_endpoint", + "Backend '%s' not available (shutdown?), cannot deserialize descriptor payload", + backend_type.c_str()); + return false; + } + + // Deserialize descriptor + RCUTILS_LOG_INFO_NAMED("deserialize_buffer_with_endpoint", "Deserializing descriptor"); + auto descriptor = ser_it->second.deserialize(cdr, endpoint_info, serialization_context); + + // Create buffer implementation with endpoint awareness + RCUTILS_LOG_INFO_NAMED("deserialize_buffer_with_endpoint", "Creating buffer from descriptor"); + auto impl_shared = ops_it->second.from_descriptor_with_endpoint(descriptor, endpoint_info); + + auto typed_impl_shared = + std::static_pointer_cast>(impl_shared); + std::unique_ptr> typed_impl_unique = + typed_impl_shared->clone(); + buffer = rosidl::Buffer(std::move(typed_impl_unique)); + return true; +} + +} // namespace rosidl_typesupport_fastrtps_cpp + +namespace eprosima +{ +namespace fastcdr +{ + +/// FastCDR serialize() function for Buffer (called by FastCDR internally) +template +inline void serialize(Cdr & cdr, const rosidl::Buffer & buffer) +{ + cdr << buffer; // Delegate to our custom operator<< +} + +/// FastCDR deserialize() function for Buffer (called by FastCDR internally) +template +inline void deserialize(Cdr & cdr, rosidl::Buffer & buffer) +{ + cdr >> buffer; // Delegate to our custom operator>> +} + +/// Serialize Buffer. +/// CPU backend: serializes directly as std::vector +/// Other backends: force-convert to CPU backend and serialize as std::vector +template +inline Cdr & operator<<(Cdr & cdr, const rosidl::Buffer & buffer) +{ + const std::string backend_type = buffer.get_backend_type(); + if (backend_type != "cpu") { + RCUTILS_LOG_INFO_NAMED("Serialize Buffer", + ("Force-converting to CPU buffer for serialization (backend: " + backend_type + ")").c_str()); + } + + // Serialize as std::vector for legacy wire compatibility. + const std::vector & vec = buffer.to_vector(); + cdr << vec; + return cdr; +} + +/// Deserialize Buffer. +/// CPU backend: deserializes directly from std::vector (fully backward compatible) +/// Other backends: use descriptor message approach +template +inline Cdr & operator>>(Cdr & cdr, rosidl::Buffer & buffer) +{ + // Only supports legacy vector-compatible CPU path. + auto original_state = cdr.get_state(); + uint32_t first_word = 0u; + cdr >> first_word; + cdr.set_state(original_state); + if (first_word == rosidl_typesupport_fastrtps_cpp::kBufferDescriptorMarker) { + throw std::runtime_error( + "Deserializing Buffer with operator>> only supports legacy CPU vector bytes"); + } + + std::vector vec; + cdr >> vec; + + // Copy into buffer (which defaults to CPU backend) + buffer.resize(vec.size()); + for (size_t i = 0; i < vec.size(); ++i) { + buffer[i] = vec[i]; + } + return cdr; +} + +} // namespace fastcdr +} // namespace eprosima + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__BUFFER_SERIALIZATION_HPP_ diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/message_type_support.h b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/message_type_support.h index 2d87c3c..df6c645 100644 --- a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/message_type_support.h +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/message_type_support.h @@ -16,11 +16,19 @@ #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP__MESSAGE_TYPE_SUPPORT_H_ #include +#include +#include #include "fastcdr/Cdr.h" +#include "rmw/topic_endpoint_info.h" #include "rosidl_runtime_c/message_type_support_struct.h" +namespace rosidl_typesupport_fastrtps_cpp +{ + struct BufferSerializationContext; +} // namespace rosidl_typesupport_fastrtps_cpp + /// Feature define to allow API version detection #define ROSIDL_TYPESUPPORT_FASTRTPS_HAS_PLAIN_TYPES @@ -113,6 +121,40 @@ typedef struct message_type_support_callbacks_t /// Pointer to the message_type_support_key_callbacks_t. /// Nullptr if the type is not keyed. message_type_support_key_callbacks_t * key_callbacks; + + /// Flag indicating if the message type contains Buffer fields. + /// Set at code generation time. + bool has_buffer_fields; + + /// Callback function for endpoint-aware message serialization + /// Only called if has_buffer_fields is true. + /** + * \param[in] untyped_ros_message Type erased pointer to message instance. + * \param [in,out] cdr Fast CDR serializer. + * \param [in] endpoint_info Endpoint info for the remote peer. + * \param [in] serialization_context RMW-owned descriptor context. + * \return true if serialization succeeded, false otherwise. + */ + bool (* cdr_serialize_with_endpoint)( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr, + const rmw_topic_endpoint_info_t & endpoint_info, + const rosidl_typesupport_fastrtps_cpp::BufferSerializationContext & serialization_context); + + /// Callback function for endpoint-aware message deserialization + /// Only called if has_buffer_fields is true. + /** + * \param [in] cdr Serialized FastCDR data object. + * \param[out] untyped_ros_message Type erased pointer to message instance. + * \param [in] endpoint_info Endpoint info for the remote peer. + * \param [in] serialization_context RMW-owned descriptor context. + * \return true if deserialization succeeded, false otherwise. + */ + bool (* cdr_deserialize_with_endpoint)( + eprosima::fastcdr::Cdr & cdr, + void * untyped_ros_message, + const rmw_topic_endpoint_info_t & endpoint_info, + const rosidl_typesupport_fastrtps_cpp::BufferSerializationContext & serialization_context); } message_type_support_callbacks_t; #endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__MESSAGE_TYPE_SUPPORT_H_ diff --git a/rosidl_typesupport_fastrtps_cpp/package.xml b/rosidl_typesupport_fastrtps_cpp/package.xml index 462d70c..e68138a 100644 --- a/rosidl_typesupport_fastrtps_cpp/package.xml +++ b/rosidl_typesupport_fastrtps_cpp/package.xml @@ -33,6 +33,7 @@ fastcdr rmw + rosidl_buffer_backend rosidl_runtime_c 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 31fe1f4..6776c3c 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -12,6 +12,7 @@ from rosidl_parser.definition import Array from rosidl_parser.definition import BasicType from rosidl_parser.definition import BoundedSequence from rosidl_parser.definition import NamespacedType +from rosidl_parser.definition import UnboundedSequence header_files = [ 'cstddef', @@ -25,6 +26,18 @@ header_files = [ 'rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp', 'fastcdr/Cdr.h', ] + +# Detect if message has Buffer fields (only uint8[] UnboundedSequence becomes Buffer) +has_buffer_fields = False +for member in message.structure.members: + if isinstance(member.type, UnboundedSequence): + # Only uint8[] arrays use Buffer + if isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'uint8': + has_buffer_fields = True + break + +if has_buffer_fields: + header_files.append('rosidl_typesupport_fastrtps_cpp/buffer_serialization.hpp') }@ @[for header_file in header_files]@ @[ if header_file in include_directives]@ @@ -108,8 +121,9 @@ namespace typesupport_fastrtps_cpp # Generates the definition for the serialization family of methods given a structure member # member: the member to serialize # suffix: the suffix name of the method. Will be used in case of recursion +# endpoint_param: parameter name for endpoint info (e.g., 'endpoint_info' or '') -def generate_member_for_cdr_serialize(member, suffix): +def generate_member_for_cdr_serialize(member, suffix, endpoint_param=''): from rosidl_generator_cpp import msg_type_only_to_cpp from rosidl_generator_cpp import msg_type_to_cpp from rosidl_parser.definition import AbstractGenericString @@ -120,8 +134,31 @@ def generate_member_for_cdr_serialize(member, suffix): from rosidl_parser.definition import BasicType from rosidl_parser.definition import BoundedSequence from rosidl_parser.definition import NamespacedType + from rosidl_parser.definition import UnboundedSequence strlist = [] strlist.append('// Member: %s' % (member.name)) + + # Handle serialization for Buffer fields (only uint8[] UnboundedSequence -> Buffer) + if isinstance(member.type, UnboundedSequence): + if isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'uint8': + if suffix == '_with_endpoint': + # Endpoint-aware serialization with backend descriptors + strlist.append('{') + strlist.append(' rosidl_typesupport_fastrtps_cpp::serialize_buffer_with_endpoint(') + strlist.append( + ' cdr, ros_message.%s, %s, serialization_context);' % + (member.name, endpoint_param)) + strlist.append('}') + return strlist + else: + # Regular CDR: use to_vector() which works for all backends (CPU, demo, etc.) + strlist.append('{') + strlist.append(' std::vector<%s> vec = ros_message.%s.to_vector();' % (msg_type_only_to_cpp(member.type.value_type), member.name)) + strlist.append(' cdr << vec;') + strlist.append('}') + return strlist + + nested_msg_suffix = '' if suffix == '_with_endpoint' else suffix if isinstance(member.type, AbstractNestedType): strlist.append('{') if isinstance(member.type, Array): @@ -130,7 +167,7 @@ def generate_member_for_cdr_serialize(member, suffix): else: strlist.append(' for (size_t i = 0; i < %d; i++) {' % (member.type.size)) if isinstance(member.type.value_type, NamespacedType): - strlist.append(' %s::typesupport_fastrtps_cpp::cdr_serialize%s(' % (('::'.join(member.type.value_type.namespaces)), suffix)) + strlist.append(' %s::typesupport_fastrtps_cpp::cdr_serialize%s(' % (('::'.join(member.type.value_type.namespaces)), nested_msg_suffix)) strlist.append(' ros_message.%s[i],' % (member.name)) strlist.append(' cdr);') else: @@ -162,7 +199,7 @@ def generate_member_for_cdr_serialize(member, suffix): elif not isinstance(member.type.value_type, NamespacedType): strlist.append(' cdr << ros_message.%s[i];' % (member.name)) else: - strlist.append(' %s::typesupport_fastrtps_cpp::cdr_serialize%s(' % (('::'.join(member.type.value_type.namespaces)), suffix)) + strlist.append(' %s::typesupport_fastrtps_cpp::cdr_serialize%s(' % (('::'.join(member.type.value_type.namespaces)), nested_msg_suffix)) strlist.append(' ros_message.%s[i],' % (member.name)) strlist.append(' cdr);') strlist.append(' }') @@ -178,7 +215,7 @@ def generate_member_for_cdr_serialize(member, suffix): elif not isinstance(member.type, NamespacedType): strlist.append('cdr << ros_message.%s;' % (member.name)) else: - strlist.append('%s::typesupport_fastrtps_cpp::cdr_serialize%s(' % (('::'.join(member.type.namespaces)), suffix)) + strlist.append('%s::typesupport_fastrtps_cpp::cdr_serialize%s(' % (('::'.join(member.type.namespaces)), nested_msg_suffix)) strlist.append(' ros_message.%s,' % (member.name)) strlist.append(' cdr);') return strlist @@ -307,6 +344,149 @@ cdr_deserialize( return true; } // NOLINT(readability/fn_size) +@[if has_buffer_fields]@ +// Endpoint-aware serialization for Buffer message types +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +cdr_serialize_with_endpoint( + const @('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])) & ros_message, + eprosima::fastcdr::Cdr & cdr, + const rmw_topic_endpoint_info_t & endpoint_info, + const rosidl_typesupport_fastrtps_cpp::BufferSerializationContext & serialization_context) +{ + try { + // Serialize all fields, using endpoint-aware serialization for Buffer fields +@[for member in message.structure.members]@ +@[ for line in generate_member_for_cdr_serialize(member, '_with_endpoint', 'endpoint_info')]@ + @(line) +@[ end for]@ + +@[end for]@ + } catch (const std::exception & e) { + RCUTILS_LOG_ERROR_NAMED( + "@(package_name).typesupport_fastrtps_cpp", + "cdr_serialize_with_endpoint failed: %s", e.what()); + return false; + } + return true; +} + +// Endpoint-aware deserialization for Buffer message types +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +cdr_deserialize_with_endpoint( + eprosima::fastcdr::Cdr & cdr, + @('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])) & ros_message, + const rmw_topic_endpoint_info_t & endpoint_info, + const rosidl_typesupport_fastrtps_cpp::BufferSerializationContext & serialization_context) +{ + // Deserialize all fields, using endpoint-aware deserialization for Buffer fields (UnboundedSequence) +@[for member in message.structure.members]@ + // Member: @(member.name) +@[ if isinstance(member.type, UnboundedSequence) and isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'uint8']@ + { + if (!rosidl_typesupport_fastrtps_cpp::deserialize_buffer_with_endpoint( + cdr, ros_message.@(member.name), endpoint_info, serialization_context)) + { + RCUTILS_LOG_ERROR_NAMED( + "@(package_name).typesupport_fastrtps_cpp", + "cdr_deserialize_with_endpoint: failed to deserialize '@(member.name)'"); + return false; + } + } +@[ elif isinstance(member.type, AbstractNestedType)]@ + { +@[ if isinstance(member.type, Array)]@ +@[ if not isinstance(member.type.value_type, (NamespacedType, AbstractWString))]@ + cdr >> ros_message.@(member.name); +@[ else]@ + for (size_t i = 0; i < @(member.type.size); i++) { +@[ if isinstance(member.type.value_type, NamespacedType)]@ + @('::'.join(member.type.value_type.namespaces))::typesupport_fastrtps_cpp::cdr_deserialize( + cdr, + ros_message.@(member.name)[i]); +@[ else]@ + bool succeeded = rosidl_typesupport_fastrtps_cpp::cdr_deserialize(cdr, ros_message.@(member.name)[i]); + if (!succeeded) { + fprintf(stderr, "failed to deserialize u16string\n"); + return false; + } +@[ end if]@ + } +@[ end if]@ +@[ else]@ +@[ if not isinstance(member.type.value_type, (NamespacedType, AbstractWString)) and not isinstance(member.type, BoundedSequence)]@ + cdr >> ros_message.@(member.name); +@[ else]@ + uint32_t cdrSize; + cdr >> cdrSize; + size_t size = static_cast(cdrSize); + + // Check there are at least 'size' remaining bytes in the CDR stream before resizing + auto old_state = cdr.get_state(); + bool correct_size = cdr.jump(size); + cdr.set_state(old_state); + if (!correct_size) { + fprintf(stderr, "sequence size exceeds remaining buffer\n"); + return false; + } + + ros_message.@(member.name).resize(size); +@[ if isinstance(member.type, BoundedSequence)]@ + if (size > @(member.type.maximum_size)) { + throw std::runtime_error("vector size exceeds upper bound"); + } +@[ end if]@ +@[ if isinstance(member.type.value_type, BasicType) and member.type.value_type.typename not in ('boolean', 'wchar')]@ + if (size > 0) { + cdr.deserialize_array(&(ros_message.@(member.name)[0]), size); + } +@[ else]@ + for (size_t i = 0; i < size; i++) { +@[ if isinstance(member.type.value_type, NamespacedType)]@ + @('::'.join(member.type.value_type.namespaces))::typesupport_fastrtps_cpp::cdr_deserialize( + cdr, + ros_message.@(member.name)[i]); +@[ else]@ + bool succeeded = rosidl_typesupport_fastrtps_cpp::cdr_deserialize(cdr, ros_message.@(member.name)[i]); + if (!succeeded) { + fprintf(stderr, "failed to deserialize u16string\n"); + return false; + } +@[ end if]@ + } +@[ end if]@ +@[ end if]@ +@[ end if]@ + } +@[ elif isinstance(member.type, BasicType) and member.type.typename == 'boolean']@ + cdr >> ros_message.@(member.name); +@[ elif isinstance(member.type, BasicType) and member.type.typename == 'wchar']@ + { + uint16_t wchar_value; + cdr >> wchar_value; + ros_message.@(member.name) = static_cast(wchar_value); + } +@[ elif isinstance(member.type, AbstractWString)]@ + { + bool succeeded = rosidl_typesupport_fastrtps_cpp::cdr_deserialize(cdr, ros_message.@(member.name)); + if (!succeeded) { + fprintf(stderr, "failed to deserialize u16string\n"); + return false; + } + } +@[ elif not isinstance(member.type, NamespacedType)]@ + cdr >> ros_message.@(member.name); +@[ else]@ + @('::'.join(member.type.namespaces))::typesupport_fastrtps_cpp::cdr_deserialize( + cdr, + ros_message.@(member.name)); +@[ end if]@ + +@[end for]@ + return true; +} +@[end if]@ @{ # Generates the definition for the get_serialized_size family of methods given a structure member @@ -323,11 +503,21 @@ def generate_member_for_get_serialized_size(member, suffix): from rosidl_parser.definition import Array from rosidl_parser.definition import BasicType from rosidl_parser.definition import BoundedSequence + from rosidl_parser.definition import UnboundedSequence from rosidl_parser.definition import NamespacedType strlist = [] strlist.append('// Member: %s' % (member.name)) if isinstance(member.type, AbstractNestedType): + # Special handling for uint8[] UnboundedSequence which becomes Buffer + if isinstance(member.type, UnboundedSequence) and isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'uint8': + # uint8[] UnboundedSequence fields are now Buffer in rosidl_generator_cpp + # Call the Buffer-specific serialization size function + strlist.append('current_alignment +=') + strlist.append(' rosidl_typesupport_fastrtps_cpp::get_buffer_serialized_size(') + strlist.append(' ros_message.%s, current_alignment);' % (member.name)) + return strlist + strlist.append('{') if isinstance(member.type, Array): strlist.append(' size_t array_size = %d;' % (member.type.size)) @@ -725,6 +915,34 @@ static size_t _@(message.structure.namespaced_type.name)__max_serialized_size(ch return ret_val; } +@[if has_buffer_fields]@ +// Endpoint-aware serialization wrapper +static bool _@(message.structure.namespaced_type.name)__cdr_serialize_with_endpoint( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr, + const rmw_topic_endpoint_info_t & endpoint_info, + const rosidl_typesupport_fastrtps_cpp::BufferSerializationContext & serialization_context) +{ + auto typed_message = + static_cast( + untyped_ros_message); + return cdr_serialize_with_endpoint(*typed_message, cdr, endpoint_info, serialization_context); +} + +// Endpoint-aware deserialization wrapper +static bool _@(message.structure.namespaced_type.name)__cdr_deserialize_with_endpoint( + eprosima::fastcdr::Cdr & cdr, + void * untyped_ros_message, + const rmw_topic_endpoint_info_t & endpoint_info, + const rosidl_typesupport_fastrtps_cpp::BufferSerializationContext & serialization_context) +{ + auto typed_message = + static_cast<@('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])) *>( + untyped_ros_message); + return cdr_deserialize_with_endpoint(cdr, *typed_message, endpoint_info, serialization_context); +} +@[end if]@ + static message_type_support_callbacks_t _@(message.structure.namespaced_type.name)__callbacks = { "@('::'.join([package_name] + list(interface_path.parents[0].parts)))", "@(message.structure.namespaced_type.name)", @@ -733,8 +951,16 @@ static message_type_support_callbacks_t _@(message.structure.namespaced_type.nam _@(message.structure.namespaced_type.name)__get_serialized_size, _@(message.structure.namespaced_type.name)__max_serialized_size, @[ if message.structure.has_any_member_with_annotation('key') ]@ - &_@(message.structure.namespaced_type.name)__key_callbacks + &_@(message.structure.namespaced_type.name)__key_callbacks, +@[ else]@ + nullptr, +@[ end if]@ + @('true' if has_buffer_fields else 'false'), +@[ if has_buffer_fields]@ + _@(message.structure.namespaced_type.name)__cdr_serialize_with_endpoint, + _@(message.structure.namespaced_type.name)__cdr_deserialize_with_endpoint @[ else]@ + nullptr, nullptr @[ end if]@ }; diff --git a/rosidl_typesupport_fastrtps_cpp/test/test_buffer_wire_compat.cpp b/rosidl_typesupport_fastrtps_cpp/test/test_buffer_wire_compat.cpp new file mode 100644 index 0000000..b2377f5 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/test/test_buffer_wire_compat.cpp @@ -0,0 +1,114 @@ +// Copyright 2026 Open Source Robotics Foundation, Inc. +// +// 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. + +#include + +#include +#include +#include +#include + +#include "fastcdr/Cdr.h" +#include "fastcdr/FastBuffer.h" +#include "rosidl_buffer/buffer.hpp" +#include "rmw/topic_endpoint_info.h" +#include "rosidl_typesupport_fastrtps_cpp/buffer_serialization.hpp" + +namespace +{ + +std::vector serialize_to_bytes(const std::function & fn) +{ + std::array raw{}; + eprosima::fastcdr::FastBuffer fast_buffer(raw.data(), raw.size()); + eprosima::fastcdr::Cdr cdr(fast_buffer); + fn(cdr); + + const auto serialized_len = cdr.get_serialized_data_length(); + return std::vector( + reinterpret_cast(raw.data()), + reinterpret_cast(raw.data()) + serialized_len); +} + +} // namespace + +TEST(BufferWireCompat, CpuBufferSerializationMatchesLegacyVectorBytes) +{ + const std::vector payload{1, 2, 3, 4, 5, 6, 7, 8}; + + rosidl::Buffer buffer; + buffer.resize(payload.size()); + for (size_t i = 0; i < payload.size(); ++i) { + buffer[i] = payload[i]; + } + + const auto endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + rosidl_typesupport_fastrtps_cpp::BufferSerializationContext serialization_context; + + const auto buffer_bytes = serialize_to_bytes( + [&](eprosima::fastcdr::Cdr & cdr) { + rosidl_typesupport_fastrtps_cpp::serialize_buffer_with_endpoint( + cdr, buffer, endpoint_info, serialization_context); + }); + + const auto vector_bytes = serialize_to_bytes( + [&](eprosima::fastcdr::Cdr & cdr) { + cdr << payload; + }); + + EXPECT_EQ(buffer_bytes, vector_bytes); +} + +TEST(BufferWireCompat, DeserializeLegacyVectorBytesIntoCpuBuffer) +{ + const std::vector payload{11, 22, 33, 44, 55}; + auto bytes = serialize_to_bytes( + [&](eprosima::fastcdr::Cdr & cdr) { + cdr << payload; + }); + + eprosima::fastcdr::FastBuffer fast_buffer( + reinterpret_cast(bytes.data()), bytes.size()); + eprosima::fastcdr::Cdr cdr(fast_buffer); + rosidl::Buffer output; + const auto endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + rosidl_typesupport_fastrtps_cpp::BufferSerializationContext serialization_context; + + rosidl_typesupport_fastrtps_cpp::deserialize_buffer_with_endpoint( + cdr, output, endpoint_info, serialization_context); + + EXPECT_EQ(output.get_backend_type(), "cpu"); + EXPECT_EQ(output.to_vector(), payload); +} + +TEST(BufferWireCompat, DescriptorMarkerIsNotInterpretedAsLegacyVector) +{ + auto bytes = serialize_to_bytes( + [&](eprosima::fastcdr::Cdr & cdr) { + cdr << static_cast(rosidl_typesupport_fastrtps_cpp::kBufferDescriptorMarker); + cdr << std::string("demo"); + }); + + eprosima::fastcdr::FastBuffer fast_buffer( + reinterpret_cast(bytes.data()), bytes.size()); + eprosima::fastcdr::Cdr cdr(fast_buffer); + rosidl::Buffer output; + const auto endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + rosidl_typesupport_fastrtps_cpp::BufferSerializationContext serialization_context; + + bool result = rosidl_typesupport_fastrtps_cpp::deserialize_buffer_with_endpoint( + cdr, output, endpoint_info, serialization_context); + EXPECT_FALSE(result) << + "Expected descriptor path deserialization to fail for unregistered backend"; +}