diff --git a/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/identifier.h b/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/identifier.h index 4711666..3fc7ba2 100644 --- a/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/identifier.h +++ b/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/identifier.h @@ -26,6 +26,10 @@ extern "C" ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC extern const char * rosidl_typesupport_fastrtps_c__identifier; +/// String identifier specific to rosidl_typesupport_fastrtps_c_v2 +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC +extern const char * rosidl_typesupport_fastrtps_c__identifier_v2; + #if __cplusplus } #endif diff --git a/rosidl_typesupport_fastrtps_c/resource/msg__rosidl_typesupport_fastrtps_c.h.em b/rosidl_typesupport_fastrtps_c/resource/msg__rosidl_typesupport_fastrtps_c.h.em index 1f69191..9d72a5f 100644 --- a/rosidl_typesupport_fastrtps_c/resource/msg__rosidl_typesupport_fastrtps_c.h.em +++ b/rosidl_typesupport_fastrtps_c/resource/msg__rosidl_typesupport_fastrtps_c.h.em @@ -1,10 +1,19 @@ @# Included from rosidl_typesupport_fastrtps_c/resource/idl__rosidl_typesupport_fastrtps_c.h.em @{ + +from rosidl_pycommon import convert_camel_case_to_lower_case_underscore + +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) + header_files = [ 'stddef.h', 'rosidl_runtime_c/message_type_support_struct.h', 'rosidl_typesupport_interface/macros.h', package_name + '/msg/rosidl_typesupport_fastrtps_c__visibility_control.h', + include_base + '__struct.h', + 'fastcdr/Cdr.h', ] }@ @[for header_file in header_files]@ @@ -26,6 +35,16 @@ extern "C" { #endif +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +bool cdr_serialize_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + const @('__'.join(message.structure.namespaced_type.namespaced_name())) * ros_message, + eprosima::fastcdr::Cdr & cdr); + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +bool cdr_deserialize_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + eprosima::fastcdr::Cdr &, + @('__'.join(message.structure.namespaced_type.namespaced_name())) * ros_message); + ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) size_t get_serialized_size_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( const void * untyped_ros_message, @@ -37,6 +56,27 @@ size_t max_serialized_size_@('__'.join([package_name] + list(interface_path.pare bool & is_plain, size_t current_alignment); +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +bool cdr_serialize_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + const @('__'.join(message.structure.namespaced_type.namespaced_name())) * ros_message, + eprosima::fastcdr::Cdr & cdr); + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +bool cdr_deserialize_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + eprosima::fastcdr::Cdr &, + @('__'.join(message.structure.namespaced_type.namespaced_name())) * ros_message); + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +size_t get_serialized_size_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + const void * untyped_ros_message, + size_t current_alignment); + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +size_t max_serialized_size_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + bool & full_bounded, + bool & is_plain, + size_t current_alignment); + ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) const rosidl_message_type_support_t * ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_c, @(', '.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])))(); 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 4b1c8f7..a44a186 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 @@ -136,6 +136,21 @@ for member in message.structure.members: forward_declares[key].add(member.name) }@ @[for key in sorted(forward_declares.keys())]@ + +@[ if key[0] != package_name]@ +ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@(package_name) +@[ end if]@ +bool cdr_serialize_@('__'.join(key))( + const @('__'.join(key)) * ros_message, + eprosima::fastcdr::Cdr & cdr); + +@[ if key[0] != package_name]@ +ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@(package_name) +@[ end if]@ +bool cdr_deserialize_@('__'.join(key))( + eprosima::fastcdr::Cdr & cdr, + @('__'.join(key)) * ros_message); + @[ if key[0] != package_name]@ ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@(package_name) @[ end if]@ @@ -151,6 +166,35 @@ size_t max_serialized_size_@('__'.join(key))( bool & is_plain, size_t current_alignment); +@[ if key[0] != package_name]@ +ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@(package_name) +@[ end if]@ +bool cdr_serialize_key_@('__'.join(key))( + const @('__'.join(key)) * ros_message, + eprosima::fastcdr::Cdr & cdr); + +@[ if key[0] != package_name]@ +ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@(package_name) +@[ end if]@ +bool cdr_deserialize_key_@('__'.join(key))( + eprosima::fastcdr::Cdr & cdr, + @('__'.join(key)) * ros_message); + +@[ if key[0] != package_name]@ +ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@(package_name) +@[ end if]@ +size_t get_serialized_size_key_@('__'.join(key))( + const void * untyped_ros_message, + size_t current_alignment); + +@[ if key[0] != package_name]@ +ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@(package_name) +@[ end if]@ +size_t max_serialized_size_key_@('__'.join(key))( + bool & full_bounded, + bool & is_plain, + size_t current_alignment); + @[ if key[0] != package_name]@ ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@(package_name) @[ end if]@ @@ -162,135 +206,133 @@ const rosidl_message_type_support_t * using _@(message.structure.namespaced_type.name)__ros_msg_type = @('__'.join(message.structure.namespaced_type.namespaced_name())); -static bool _@(message.structure.namespaced_type.name)__cdr_serialize( - const void * untyped_ros_message, - eprosima::fastcdr::Cdr & cdr) -{ - if (!untyped_ros_message) { - fprintf(stderr, "ros message handle is null\n"); - return false; - } - const _@(message.structure.namespaced_type.name)__ros_msg_type * ros_message = static_cast(untyped_ros_message); -@[for member in message.structure.members]@ - // Field name: @(member.name) - { @{ -type_ = member.type -if isinstance(type_, AbstractNestedType): + +# 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 + +def generate_member_for_cdr_serialize(member, suffix): + 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 + 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 + 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, 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: + strlist.append(' size_t size = ros_message->%s.size;' % (member.name)) + strlist.append(' auto array_ptr = ros_message->%s.data;' % (member.name)) + if isinstance(member.type, BoundedSequence): + strlist.append(' if (size > %d) {' % (member.type.maximum_size)) + strlist.append(' fprintf(stderr, \"array size exceeds upper bound\\n\");') + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' cdr << static_cast(size);') + if isinstance(member.type.value_type, AbstractString): + strlist.append(' for (size_t i = 0; i < size; ++i) {') + strlist.append(' const rosidl_runtime_c__String * str = &array_ptr[i];') + strlist.append(' if (str->capacity == 0 || str->capacity <= str->size) {') + strlist.append(' fprintf(stderr, \"string capacity not greater than size\\n\");') + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' if (str->data[str->size] != \'\\0\') {') + strlist.append(' fprintf(stderr, \"string not null-terminated\\n\");') + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' cdr << str->data;') + 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(' const rosidl_runtime_c__U16String * str = &array_ptr[i];') + strlist.append(' if (str->capacity == 0 || str->capacity <= str->size) {') + strlist.append(' fprintf(stderr, \"string capacity not greater than size\\n\");') + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' if (str->data[str->size] != \'\\0\') {') + strlist.append(' fprintf(stderr, \"string not null-terminated\\n\");') + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' rosidl_typesupport_fastrtps_c::u16string_to_wstring(*str, wstr);') + strlist.append(' cdr << wstr;') + 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(' cdr_serialize%s_%s(' % (suffix, ('__'.join(member.type.value_type.namespaced_name())))) + strlist.append(' static_cast(&array_ptr[i]), cdr);') + strlist.append(' }') + elif isinstance(member.type.value_type, BasicType): + strlist.append(' cdr.serializeArray(array_ptr, size);') + else : + strlist.append(' for (size_t i = 0; i < size; ++i) {') + strlist.append(' cdr_serialize%s_%s(' % (suffix, ('__'.join(member.type.value_type.namespaced_name())))) + strlist.append(' &array_ptr[i], cdr);') + strlist.append(' }') + elif isinstance(member.type, AbstractString): + strlist.append(' const rosidl_runtime_c__String * str = &ros_message->%s;' % (member.name)) + strlist.append(' if (str->capacity == 0 || str->capacity <= str->size) {') + strlist.append(' fprintf(stderr, \"string capacity not greater than size\\n\");') + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' if (str->data[str->size] != \'\\0\') {') + strlist.append(' fprintf(stderr, \"string not null-terminated\\n\");') + strlist.append(' return false;') + strlist.append(' }') + strlist.append(' cdr << str->data;') + elif isinstance(member.type, AbstractWString): + strlist.append(' std::wstring wstr;') + strlist.append(' rosidl_typesupport_fastrtps_c::u16string_to_wstring(ros_message->%s, wstr);' % (member.name)) + strlist.append(' cdr << wstr;') + elif isinstance(member.type, BasicType) and member.type.typename == 'boolean': + strlist.append(' cdr << (ros_message->%s ? true : false);' % (member.name)) + elif isinstance(member.type, BasicType) and member.type.typename == 'wchar': + strlist.append(' cdr << static_cast(ros_message->%s);' % (member.name)) + elif isinstance(member.type, BasicType): + strlist.append(' cdr << ros_message->%s;' % (member.name)) + else: + strlist.append(' cdr_serialize%s_%s(' % (suffix, ('__'.join(member.type.namespaced_name())))) + strlist.append(' &ros_message->%s, cdr);' % (member.name)) + strlist.append('}') + + return strlist }@ -@[ if isinstance(type_, NamespacedType)]@ - const message_type_support_callbacks_t * callbacks = - static_cast( - ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( - rosidl_typesupport_fastrtps_c, @(', '.join(type_.namespaced_name())) - )()->data); -@[ end if]@ -@[ if isinstance(member.type, AbstractNestedType)]@ -@[ if isinstance(member.type, Array)]@ - size_t size = @(member.type.size); - auto array_ptr = ros_message->@(member.name); -@[ else]@ - size_t size = ros_message->@(member.name).size; - auto array_ptr = ros_message->@(member.name).data; -@[ if isinstance(member.type, BoundedSequence)]@ - if (size > @(member.type.maximum_size)) { - fprintf(stderr, "array size exceeds upper bound\n"); - return false; - } -@[ end if]@ - cdr << static_cast(size); -@[ end if]@ -@[ if isinstance(member.type.value_type, AbstractString)]@ - for (size_t i = 0; i < size; ++i) { - const rosidl_runtime_c__String * str = &array_ptr[i]; - if (str->capacity == 0 || str->capacity <= str->size) { - fprintf(stderr, "string capacity not greater than size\n"); - return false; - } - if (str->data[str->size] != '\0') { - fprintf(stderr, "string not null-terminated\n"); - return false; - } - cdr << str->data; - } -@[ elif isinstance(member.type.value_type, AbstractWString)]@ - std::wstring wstr; - for (size_t i = 0; i < size; ++i) { - const rosidl_runtime_c__U16String * str = &array_ptr[i]; - if (str->capacity == 0 || str->capacity <= str->size) { - fprintf(stderr, "string capacity not greater than size\n"); - return false; - } - if (str->data[str->size] != u'\0') { - fprintf(stderr, "string not null-terminated\n"); - return false; - } - rosidl_typesupport_fastrtps_c::u16string_to_wstring(*str, wstr); - cdr << wstr; - } -@[ elif isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'wchar']@ - for (size_t i = 0; i < size; ++i) { - if (!callbacks->cdr_serialize( - static_cast(&array_ptr[i]), cdr)) - { - return false; - } - } -@[ elif isinstance(member.type.value_type, BasicType)]@ - cdr.serializeArray(array_ptr, size); -@[ else]@ - for (size_t i = 0; i < size; ++i) { - if (!callbacks->cdr_serialize( - &array_ptr[i], cdr)) - { - return false; - } - } -@[ end if]@ -@[ elif isinstance(member.type, AbstractString)]@ - const rosidl_runtime_c__String * str = &ros_message->@(member.name); - if (str->capacity == 0 || str->capacity <= str->size) { - fprintf(stderr, "string capacity not greater than size\n"); - return false; - } - if (str->data[str->size] != '\0') { - fprintf(stderr, "string not null-terminated\n"); - return false; - } - cdr << str->data; -@[ elif isinstance(member.type, AbstractWString)]@ - std::wstring wstr; - rosidl_typesupport_fastrtps_c::u16string_to_wstring(ros_message->@(member.name), wstr); - cdr << wstr; -@[ elif isinstance(member.type, BasicType) and member.type.typename == 'boolean']@ - cdr << (ros_message->@(member.name) ? true : false); -@[ elif isinstance(member.type, BasicType) and member.type.typename == 'wchar']@ - cdr << static_cast(ros_message->@(member.name)); -@[ elif isinstance(member.type, BasicType)]@ - cdr << ros_message->@(member.name); -@[ else]@ - if (!callbacks->cdr_serialize( - &ros_message->@(member.name), cdr)) - { - return false; - } -@[ end if]@ - } + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +bool cdr_serialize_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + const @('__'.join(message.structure.namespaced_type.namespaced_name())) * ros_message, + eprosima::fastcdr::Cdr & cdr) +{ +@[for member in message.structure.members]@ +@[ for line in generate_member_for_cdr_serialize(member, '')]@ + @(line) +@[ end for]@ @[end for]@ return true; } -static bool _@(message.structure.namespaced_type.name)__cdr_deserialize( +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +bool cdr_deserialize_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( eprosima::fastcdr::Cdr & cdr, - void * untyped_ros_message) + @('__'.join(message.structure.namespaced_type.namespaced_name())) * ros_message) { - if (!untyped_ros_message) { - fprintf(stderr, "ros message handle is null\n"); - return false; - } - _@(message.structure.namespaced_type.name)__ros_msg_type * ros_message = static_cast<_@(message.structure.namespaced_type.name)__ros_msg_type *>(untyped_ros_message); @[for member in message.structure.members]@ // Field name: @(member.name) { @@ -299,13 +341,6 @@ type_ = member.type if isinstance(type_, AbstractNestedType): type_ = type_.value_type }@ -@[ if isinstance(type_, NamespacedType)]@ - const message_type_support_callbacks_t * callbacks = - static_cast( - ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( - rosidl_typesupport_fastrtps_c, @(', '.join(type_.namespaced_name())) - )()->data); -@[ end if]@ @[ if isinstance(member.type, AbstractNestedType)]@ @[ if isinstance(member.type, Array)]@ size_t size = @(member.type.size); @@ -386,11 +421,7 @@ else: cdr.deserializeArray(array_ptr, size); @[ else]@ for (size_t i = 0; i < size; ++i) { - if (!callbacks->cdr_deserialize( - cdr, &array_ptr[i])) - { - return false; - } + cdr_deserialize_@('__'.join(member.type.value_type.namespaced_name()))(cdr, &array_ptr[i]); } @[ end if]@ @[ elif isinstance(member.type, AbstractString)]@ @@ -429,11 +460,7 @@ else: @[ elif isinstance(member.type, BasicType)]@ cdr >> ros_message->@(member.name); @[ else]@ - if (!callbacks->cdr_deserialize( - cdr, &ros_message->@(member.name))) - { - return false; - } + cdr_deserialize_@('__'.join(member.type.namespaced_name()))(cdr, &ros_message->@(member.name)); @[ end if]@ } @@ -441,6 +468,74 @@ else: return true; } // NOLINT(readability/fn_size) +@{ + +# Generates the definition for the get_serialized_size 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 + +def generate_member_for_get_serialized_size(member, suffix): + 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 + 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 + strlist = [] + strlist.append('// Field name: %s' % (member.name)) + if isinstance(member.type, AbstractNestedType): + strlist.append('{') + if isinstance(member.type, Array): + strlist.append(' size_t array_size = %d;' % (member.type.size)) + strlist.append(' auto array_ptr = ros_message->%s;' % (member.name)) + else: + strlist.append(' size_t array_size = ros_message->%s.size;' % (member.name)) + strlist.append(' auto array_ptr = ros_message->%s.data;' % (member.name)) + strlist.append(' current_alignment += padding +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, padding);') + if isinstance(member.type.value_type, AbstractGenericString): + strlist.append(' for (size_t index = 0; index < array_size; ++index) {') + strlist.append(' current_alignment += padding +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + ') + if isinstance(member.type.value_type, AbstractWString): + strlist.append(' wchar_size *') + strlist.append(' (array_ptr[index].size + 1);') + strlist.append(' }') + elif isinstance(member.type.value_type, BasicType): + strlist.append(' (void)array_ptr;') + strlist.append(' size_t item_size = sizeof(array_ptr[0]);') + strlist.append(' current_alignment += array_size * item_size +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, item_size);') + else: + strlist.append(' for (size_t index = 0; index < array_size; ++index) {') + strlist.append(' current_alignment += get_serialized_size%s_%s(' % (suffix, ('__'.join(member.type.value_type.namespaced_name())))) + strlist.append(' &array_ptr[index], current_alignment);') + strlist.append(' }') + strlist.append('}') + else: + if isinstance(member.type, AbstractGenericString): + strlist.append('current_alignment += padding +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, padding) +') + if isinstance(member.type, AbstractWString): + strlist.append(' wchar_size *') + strlist.append(' (ros_message->%s.size + 1);' % (member.name)) + elif isinstance(member.type, BasicType): + strlist.append('{') + strlist.append(' size_t item_size = sizeof(ros_message->%s);' % (member.name)) + strlist.append(' current_alignment += item_size +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, item_size);') + strlist.append('}') + else: + strlist.append(' current_alignment += get_serialized_size%s_%s(' % (suffix, ('__'.join(member.type.namespaced_name())))) + strlist.append(' &(ros_message->%s), current_alignment);' % (member.name)) + return strlist +}@ + ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) size_t get_serialized_size_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( const void * untyped_ros_message, @@ -456,69 +551,106 @@ size_t get_serialized_size_@('__'.join([package_name] + list(interface_path.pare (void)wchar_size; @[for member in message.structure.members]@ - // field.name @(member.name) -@[ if isinstance(member.type, AbstractNestedType)]@ - { -@[ if isinstance(member.type, Array)]@ - size_t array_size = @(member.type.size); - auto array_ptr = ros_message->@(member.name); -@[ else]@ - size_t array_size = ros_message->@(member.name).size; - auto array_ptr = ros_message->@(member.name).data; - current_alignment += padding + - eprosima::fastcdr::Cdr::alignment(current_alignment, padding); -@[ 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]@ - (array_ptr[index].size + 1); - } -@[ elif isinstance(member.type.value_type, BasicType)]@ - (void)array_ptr; - size_t item_size = sizeof(array_ptr[0]); - current_alignment += array_size * item_size + - eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); -@[ else] - for (size_t index = 0; index < array_size; ++index) { - current_alignment += get_serialized_size_@('__'.join(member.type.value_type.namespaced_name()))( - &array_ptr[index], current_alignment); - } -@[ end if]@ - } -@[ 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); -@[ elif isinstance(member.type, BasicType)]@ - { - size_t item_size = sizeof(ros_message->@(member.name)); - current_alignment += item_size + - eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); - } -@[ else] - current_alignment += get_serialized_size_@('__'.join(member.type.namespaced_name()))( - &(ros_message->@(member.name)), current_alignment); -@[ end if]@ -@[ end if]@ -@[end for]@ +@[ for line in generate_member_for_get_serialized_size(member, '')]@ + @(line) +@[ end for]@ +@[end for]@ return current_alignment - initial_alignment; } -static uint32_t _@(message.structure.namespaced_type.name)__get_serialized_size(const void * untyped_ros_message) -{ - return static_cast( - get_serialized_size_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( - untyped_ros_message, 0)); -} +@{ + +# Generates the definition for the max_serialized_size 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 + +def generate_member_for_max_serialized_size(member, suffix): + 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 + 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 + strlist = [] + strlist.append('// Field name: %s' % (member.name)) + strlist.append('{') + + if isinstance(member.type, AbstractNestedType): + if isinstance(member.type, Array): + strlist.append(' size_t array_size = %d;' % (member.type.size)) + elif isinstance(member.type, BoundedSequence): + strlist.append(' size_t array_size = %d;' % (member.type.maximum_size)) + else: + strlist.append(' size_t array_size = 0;') + strlist.append(' full_bounded = false;') + if isinstance(member.type, AbstractSequence): + strlist.append(' is_plain = false;') + strlist.append(' current_alignment += padding +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, padding);') + else: + strlist.append(' size_t array_size = 1;') + + type_ = member.type + if isinstance(type_, AbstractNestedType): + type_ = type_.value_type + + if isinstance(type_, AbstractGenericString): + strlist.append(' full_bounded = false;') + strlist.append(' is_plain = false;') + strlist.append(' for (size_t index = 0; index < array_size; ++index) {') + strlist.append(' current_alignment += padding +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + ') + if type_.has_maximum_size(): + if isinstance(type_, AbstractWString): + strlist.append(' wchar_size *') + strlist.append(' %d +' % (type_.maximum_size)) + if isinstance(type_, AbstractWString): + strlist.append(' wchar_size *') + strlist.append(' 1;') + strlist.append(' }') + elif isinstance(type_, BasicType): + if type_.typename in ('boolean', 'octet', 'char', 'uint8', 'int8'): + strlist.append(' last_member_size = array_size * sizeof(uint8_t);') + strlist.append(' current_alignment += array_size * sizeof(uint8_t);') + elif type_.typename in ('wchar', 'int16', 'uint16'): + strlist.append(' last_member_size = array_size * sizeof(uint16_t);') + strlist.append(' current_alignment += array_size * sizeof(uint16_t) +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint16_t));') + elif type_.typename in ('int32', 'uint32', 'float'): + strlist.append(' last_member_size = array_size * sizeof(uint32_t);') + strlist.append(' current_alignment += array_size * sizeof(uint32_t) +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t));') + elif type_.typename in ('int64', 'uint64', 'double'): + strlist.append(' last_member_size = array_size * sizeof(uint64_t);') + strlist.append(' current_alignment += array_size * sizeof(uint64_t) +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t));') + elif type_.typename == 'long double': + strlist.append(' last_member_size = array_size * sizeof(long double);') + strlist.append(' current_alignment += array_size * sizeof(long double) +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(long double));') + else: + strlist.append(' last_member_size = 0;') + strlist.append(' for (size_t index = 0; index < array_size; ++index) {') + strlist.append(' bool inner_full_bounded;') + strlist.append(' bool inner_is_plain;') + strlist.append(' size_t inner_size;') + strlist.append(' inner_size =') + strlist.append(' max_serialized_size%s_%s(' % (suffix, ('__'.join(type_.namespaced_name())))) + strlist.append(' inner_full_bounded, inner_is_plain, current_alignment);') + strlist.append(' last_member_size += inner_size;') + strlist.append(' current_alignment += inner_size;') + strlist.append(' full_bounded &= inner_full_bounded;') + strlist.append(' is_plain &= inner_is_plain;') + strlist.append(' }') + strlist.append('}') + return strlist +}@ ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) size_t max_serialized_size_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( @@ -545,87 +677,117 @@ last_member_name_ = None @{ last_member_name_ = member.name }@ - // member: @(member.name) - { -@[ if isinstance(member.type, AbstractNestedType)]@ -@[ if isinstance(member.type, Array)]@ - size_t array_size = @(member.type.size); -@[ elif isinstance(member.type, BoundedSequence)]@ - size_t array_size = @(member.type.maximum_size); -@[ else]@ - size_t array_size = 0; - full_bounded = false; -@[ end if]@ -@[ if isinstance(member.type, AbstractSequence)]@ - is_plain = false; - current_alignment += padding + - eprosima::fastcdr::Cdr::alignment(current_alignment, padding); -@[ end if]@ -@[ else]@ - size_t array_size = 1; +@[ for line in generate_member_for_max_serialized_size(member, '')]@ + @(line) +@[ end for]@ + +@[end for]@ + + size_t ret_val = current_alignment - initial_alignment; +@[if last_member_name_ is not None]@ + if (is_plain) { + // All members are plain, and type is not empty. + // We still need to check that the in-memory alignment + // is the same as the CDR mandated alignment. + using DataType = @('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])); + is_plain = + ( + offsetof(DataType, @(last_member_name_)) + + last_member_size + ) == ret_val; + } +@[end if]@ + return ret_val; +} + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +bool cdr_serialize_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + const @('__'.join(message.structure.namespaced_type.namespaced_name())) * ros_message, + eprosima::fastcdr::Cdr & cdr) +{ +@[for member in message.structure.members]@ +@[ if not member.has_annotation('key') and message.structure.has_any_member_with_annotation('key')]@ +@[ continue]@ @[ end if]@ +@[ for line in generate_member_for_cdr_serialize(member, '_key')]@ + @(line) +@[ end for]@ + +@[end for]@ + return true; +} + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +bool cdr_deserialize_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + eprosima::fastcdr::Cdr &cdr, + @('__'.join(message.structure.namespaced_type.namespaced_name())) * ros_message) +{ + (void)ros_message; + (void)cdr; + // TODO + return false; +} + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +size_t get_serialized_size_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + const void * untyped_ros_message, + size_t current_alignment) +{ + const _@(message.structure.namespaced_type.name)__ros_msg_type * ros_message = static_cast(untyped_ros_message); + (void)ros_message; + + 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]@ +@[ if not member.has_annotation('key') and message.structure.has_any_member_with_annotation('key')]@ +@[ continue]@ +@[ end if]@ +@[ for line in generate_member_for_get_serialized_size(member, '_key')]@ + @(line) +@[ end for]@ + +@[end for]@ + return current_alignment - initial_alignment; +} + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +size_t max_serialized_size_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + bool & full_bounded, + bool & is_plain, + size_t current_alignment) +{ + size_t initial_alignment = current_alignment; + + const size_t padding = 4; + const size_t wchar_size = 4; + size_t last_member_size = 0; + (void)last_member_size; + (void)padding; + (void)wchar_size; + + full_bounded = true; + is_plain = true; @{ -type_ = member.type -if isinstance(type_, AbstractNestedType): - type_ = type_.value_type +last_member_name_ = None }@ -@[ 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; - } -@[ elif isinstance(type_, BasicType)]@ -@[ if type_.typename in ('boolean', 'octet', 'char', 'uint8', 'int8')]@ - last_member_size = array_size * sizeof(uint8_t); - current_alignment += array_size * sizeof(uint8_t); -@[ elif type_.typename in ('wchar', 'int16', '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)); -@[ elif type_.typename in ('int32', 'uint32', 'float')]@ - last_member_size = array_size * sizeof(uint32_t); - current_alignment += array_size * sizeof(uint32_t) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t)); -@[ elif type_.typename in ('int64', 'uint64', 'double')]@ - last_member_size = array_size * sizeof(uint64_t); - current_alignment += array_size * sizeof(uint64_t) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t)); -@[ elif type_.typename == 'long double']@ - last_member_size = array_size * sizeof(long double); - current_alignment += array_size * sizeof(long double) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(long double)); -@[ end if]@ -@[ else] - last_member_size = 0; - for (size_t index = 0; index < array_size; ++index) { - bool inner_full_bounded; - bool inner_is_plain; - size_t inner_size; - inner_size = - max_serialized_size_@('__'.join(type_.namespaced_name()))( - inner_full_bounded, inner_is_plain, current_alignment); - last_member_size += inner_size; - current_alignment += inner_size; - full_bounded &= inner_full_bounded; - is_plain &= inner_is_plain; - } +@[for member in message.structure.members]@ +@{ +last_member_name_ = member.name +}@ +@[ if not member.has_annotation('key') and message.structure.has_any_member_with_annotation('key')]@ +@[ continue]@ @[ end if]@ - } -@[end for]@ +@[ for line in generate_member_for_max_serialized_size(member, '_key')]@ + @(line) +@[ end for]@ +@[end for]@ size_t ret_val = current_alignment - initial_alignment; @[if last_member_name_ is not None]@ if (is_plain) { @@ -639,11 +801,104 @@ if isinstance(type_, AbstractNestedType): last_member_size ) == ret_val; } - @[end if]@ return ret_val; } +@[ if message.structure.has_any_member_with_annotation('key') ]@ +static bool _@(message.structure.namespaced_type.name)__cdr_serialize_key( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr) +{ + 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)ros_message; + return cdr_serialize_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))(ros_message, cdr); +} + +static bool _@(message.structure.namespaced_type.name)__cdr_deserialize_key( + eprosima::fastcdr::Cdr & cdr, + void * untyped_ros_message) +{ + 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)ros_message; + return cdr_deserialize_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))(cdr, ros_message); +} + +static size_t _@(message.structure.namespaced_type.name)__get_serialized_size_key( + const void * untyped_ros_message) +{ + return get_serialized_size_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + untyped_ros_message, 0); +} + +static +size_t +_@(message.structure.namespaced_type.name)__max_serialized_size_key( + bool & is_unbounded) +{ + bool full_bounded; + bool is_plain; + size_t ret_val; + + ret_val = max_serialized_size_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + full_bounded, is_plain, 0); + + is_unbounded = !full_bounded; + return ret_val; +} + +static message_type_support_key_callbacks_t __key_callbacks_@(message.structure.namespaced_type.name) = { + _@(message.structure.namespaced_type.name)__cdr_serialize_key, + _@(message.structure.namespaced_type.name)__cdr_deserialize_key, + _@(message.structure.namespaced_type.name)__get_serialized_size_key, + _@(message.structure.namespaced_type.name)__max_serialized_size_key, +}; +@[ end if]@ + +@ +@# // Collect the callback functions and provide a function to get the type support struct. + +static bool _@(message.structure.namespaced_type.name)__cdr_serialize( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr) +{ + 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)ros_message; + return cdr_serialize_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))(ros_message, cdr); +} + +static bool _@(message.structure.namespaced_type.name)__cdr_deserialize( + eprosima::fastcdr::Cdr & cdr, + void * untyped_ros_message) +{ + 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)ros_message; + return cdr_deserialize_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))(cdr, ros_message); +} + +static uint32_t _@(message.structure.namespaced_type.name)__get_serialized_size(const void * untyped_ros_message) +{ + return static_cast( + get_serialized_size_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + untyped_ros_message, 0)); +} + static size_t _@(message.structure.namespaced_type.name)__max_serialized_size(char & bounds_info) { bool full_bounded; @@ -659,20 +914,22 @@ static size_t _@(message.structure.namespaced_type.name)__max_serialized_size(ch return ret_val; } -@ -@# // 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) = { "@('::'.join([package_name] + list(interface_path.parents[0].parts)))", "@(message.structure.namespaced_type.name)", _@(message.structure.namespaced_type.name)__cdr_serialize, _@(message.structure.namespaced_type.name)__cdr_deserialize, _@(message.structure.namespaced_type.name)__get_serialized_size, - _@(message.structure.namespaced_type.name)__max_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) +@[ else]@ + nullptr +@[ end if]@ }; static rosidl_message_type_support_t _@(message.structure.namespaced_type.name)__type_support = { - rosidl_typesupport_fastrtps_c__identifier, + rosidl_typesupport_fastrtps_c__identifier_v2, &__callbacks_@(message.structure.namespaced_type.name), get_message_typesupport_handle_function, &@(idl_structure_type_to_c_typename(message.structure.namespaced_type))__@(GET_HASH_FUNC), diff --git a/rosidl_typesupport_fastrtps_c/src/identifier.cpp b/rosidl_typesupport_fastrtps_c/src/identifier.cpp index bf64666..2075db0 100644 --- a/rosidl_typesupport_fastrtps_c/src/identifier.cpp +++ b/rosidl_typesupport_fastrtps_c/src/identifier.cpp @@ -24,6 +24,9 @@ extern "C" const char * rosidl_typesupport_fastrtps_c__identifier = "rosidl_typesupport_fastrtps_c"; +const char * + rosidl_typesupport_fastrtps_c__identifier_v2 = "rosidl_typesupport_fastrtps_c_v2"; + #if defined(__cplusplus) } #endif diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/identifier.hpp b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/identifier.hpp index b9c7aef..c080631 100644 --- a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/identifier.hpp +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/identifier.hpp @@ -24,6 +24,10 @@ namespace rosidl_typesupport_fastrtps_cpp ROSIDL_TYPESUPPORT_FASTRTPS_CPP_IMPORT extern const char * typesupport_identifier; +/// String identifier specific to `rosidl_typesupport_fastrtps_cpp_v2`. +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_IMPORT +extern const char * typesupport_identifier_v2; + } // namespace rosidl_typesupport_fastrtps_cpp #endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__IDENTIFIER_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 4f37fdb..dc9d3bf 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 @@ -26,6 +26,48 @@ #define ROSIDL_TYPESUPPORT_FASTRTPS_BOUNDED_TYPE 0x01 #define ROSIDL_TYPESUPPORT_FASTRTPS_PLAIN_TYPE 0x03 +// Holds generated methods related with keys +typedef struct message_type_support_key_callbacks_t +{ + /// Callback function for key serialization + /** + * \param[in] untyped_ros_message Type erased pointer to message instance. + * \param [in,out] Serialized FastCDR data object. + * \return true if serialization succeeded, false otherwise. + */ + bool (* cdr_serialize_key)( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr); + + /// Callback function for key deserialization + /** + * \param [in] Serialized FastCDR data object. + * \param [out] untyped_ros_message Type erased pointer to message instance. + * \return true if deserialization succeeded, false otherwise. + */ + bool (* cdr_deserialize_key)( + eprosima::fastcdr::Cdr & cdr, + void * untyped_ros_message); + + /// Callback function to get size of the key data + /** + * \param [in] untyped_ros_message Type erased pointer to message instance + * \return The size of the serialized key in bytes. + */ + size_t (* get_serialized_size_key)( + const void * untyped_ros_message); + + /// Callback function to determine the maximum size needed for key serialization, + /// which is used for key type support initialization. + /** + * \param [in,out] is_unbounded Whether the key has any unbounded member. + * \return The maximum key serialized size, in bytes. + */ + size_t (* max_serialized_size_key)( + bool & is_unbounded); + +} message_type_support_key_callbacks_t; + /// Encapsulates the callbacks for getting properties of this rosidl type. /** * These callbacks are implemented in the generated sources. @@ -76,6 +118,11 @@ typedef struct message_type_support_callbacks_t * \return The maximum serialized size, in bytes. */ size_t (* max_serialized_size)(char & bounds_info); + + /// Pointer to the message_type_support_key_callbacks_t. + /// Nullptr if the type is not keyed. + message_type_support_key_callbacks_t * key_callbacks; + } message_type_support_callbacks_t; #endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__MESSAGE_TYPE_SUPPORT_H_ diff --git a/rosidl_typesupport_fastrtps_cpp/resource/msg__rosidl_typesupport_fastrtps_cpp.hpp.em b/rosidl_typesupport_fastrtps_cpp/resource/msg__rosidl_typesupport_fastrtps_cpp.hpp.em index 2b918a4..b9cae03 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__rosidl_typesupport_fastrtps_cpp.hpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__rosidl_typesupport_fastrtps_cpp.hpp.em @@ -49,6 +49,7 @@ header_files = [ @[ end if]@ #include "@(header_file)" @[end for]@ + @[for ns in message.structure.namespaced_type.namespaces]@ namespace @(ns) @@ -83,6 +84,31 @@ max_serialized_size_@(message.structure.namespaced_type.name)( bool & is_plain, size_t current_alignment); +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +cdr_serialize_key( + const @('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])) & ros_message, + eprosima::fastcdr::Cdr &); + +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +cdr_deserialize_key( + eprosima::fastcdr::Cdr & cdr, + @('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])) & ros_message); + +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +get_serialized_size_key( + const @('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])) & ros_message, + size_t current_alignment); + +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +max_serialized_size_key_@(message.structure.namespaced_type.name)( + bool & full_bounded, + bool & is_plain, + size_t current_alignment); + } // namespace typesupport_fastrtps_cpp @[ for ns in reversed(message.structure.namespaced_type.namespaces)]@ 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 c4623c5..ec4208a 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -72,6 +72,20 @@ max_serialized_size_@(type_.name)( bool & full_bounded, bool & is_plain, size_t current_alignment); +bool cdr_serialize_key( + const @('::'.join(type_.namespaced_name())) &, + eprosima::fastcdr::Cdr &); +bool cdr_deserialize_key( + eprosima::fastcdr::Cdr &, + @('::'.join(type_.namespaced_name())) &); +size_t get_serialized_size_key( + const @('::'.join(type_.namespaced_name())) &, + size_t current_alignment); +size_t +max_serialized_size_key_@(type_.name)( + bool & full_bounded, + bool & is_plain, + size_t current_alignment); } // namespace typesupport_fastrtps_cpp @[ for ns in reversed(type_.namespaces)]@ } // namespace @(ns) @@ -90,6 +104,94 @@ namespace @(ns) @{forward_declared_types.add(message.structure.namespaced_type.namespaced_name())}@ 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 + +def generate_member_for_cdr_serialize(member, suffix): + 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 + from rosidl_parser.definition import AbstractNestedType + from rosidl_parser.definition import AbstractSequence + 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 + strlist = [] + strlist.append('// Member: %s' % (member.name)) + if isinstance(member.type, AbstractNestedType): + strlist.append('{') + if isinstance(member.type, Array): + if not isinstance(member.type.value_type, (NamespacedType, AbstractWString)): + strlist.append(' cdr << ros_message.%s;' % (member.name)) + else: + if isinstance(member.type.value_type, AbstractWString): + strlist.append(' std::wstring wstr;') + 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(' ros_message.%s[i],' % (member.name)) + strlist.append(' cdr);') + else: + strlist.append(' rosidl_typesupport_fastrtps_cpp::u16string_to_wstring(ros_message.%s[i], wstr);' % (member.name)) + strlist.append(' cdr << wstr;') + strlist.append(' }') + else: + if isinstance(member.type, BoundedSequence) or isinstance(member.type.value_type, (NamespacedType, AbstractWString)): + strlist.append(' size_t size = ros_message.%s.size();' % (member.name)) + if isinstance(member.type, BoundedSequence): + strlist.append(' if (size > %d) {' % (member.type.maximum_size)) + strlist.append(' throw std::runtime_error("array size exceeds upper bound");') + strlist.append(' }') + if not isinstance(member.type.value_type, (NamespacedType, AbstractWString)) and not isinstance(member.type, BoundedSequence): + strlist.append(' cdr << ros_message.%s;' % (member.name)) + else: + strlist.append(' cdr << static_cast(size);') + if isinstance(member.type.value_type, BasicType) and member.type.value_type.typename not in ('boolean', 'wchar'): + strlist.append(' if (size > 0) {') + strlist.append(' cdr.serializeArray(&(ros_message.%s[0]), size);' % (member.name)) + strlist.append(' }') + else: + if isinstance(member.type.value_type, AbstractWString): + strlist.append(' std::wstring wstr;') + strlist.append(' for (size_t i = 0; i < size; i++) {') + if isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'boolean': + strlist.append(' cdr << (ros_message.%s[i] ? true : false);' % (member.name)) + elif isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'wchar': + strlist.append(' cdr << static_cast(ros_message.%s[i]);' % (member.name)) + elif isinstance(member.type.value_type, AbstractWString): + strlist.append(' rosidl_typesupport_fastrtps_cpp::u16string_to_wstring(ros_message.%s[i], wstr);' % (member.name)); + strlist.append(' cdr << wstr;') + 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(' ros_message.%s[i],' % (member.name)) + strlist.append(' cdr);') + strlist.append(' }') + strlist.append(' }') + elif isinstance(member.type, BasicType) and member.type.typename == 'boolean': + strlist.append(' cdr << (ros_message.%s ? true : false);' % (member.name)) + elif isinstance(member.type, BasicType) and member.type.typename == 'wchar': + strlist.append(' cdr << static_cast(ros_message.%s);' % (member.name)) + elif isinstance(member.type, AbstractWString): + strlist.append('{') + strlist.append(' std::wstring wstr;') + strlist.append(' rosidl_typesupport_fastrtps_cpp::u16string_to_wstring(ros_message.%s, wstr);' % (member.name)) + strlist.append(' cdr << wstr;') + strlist.append('}') + 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(' ros_message.%s,' % (member.name)) + strlist.append(' cdr);') + return strlist +}@ bool ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) @@ -98,85 +200,10 @@ cdr_serialize( eprosima::fastcdr::Cdr & cdr) { @[for member in message.structure.members]@ - // Member: @(member.name) -@[ if isinstance(member.type, AbstractNestedType)]@ - { -@[ if isinstance(member.type, Array)]@ -@[ if not isinstance(member.type.value_type, (NamespacedType, AbstractWString))]@ - cdr << ros_message.@(member.name); -@[ else]@ -@[ if isinstance(member.type.value_type, AbstractWString)]@ - std::wstring wstr; -@[ end if]@ - 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_serialize( - ros_message.@(member.name)[i], - cdr); -@[ else]@ - rosidl_typesupport_fastrtps_cpp::u16string_to_wstring(ros_message.@(member.name)[i], wstr); - cdr << wstr; -@[ end if]@ - } -@[ end if]@ -@[ else]@ -@[ if isinstance(member.type, BoundedSequence) or isinstance(member.type.value_type, (NamespacedType, AbstractWString))]@ - size_t size = ros_message.@(member.name).size(); -@[ if isinstance(member.type, BoundedSequence)]@ - if (size > @(member.type.maximum_size)) { - throw std::runtime_error("array size exceeds upper bound"); - } -@[ end if]@ -@[ end if]@ -@[ if not isinstance(member.type.value_type, (NamespacedType, AbstractWString)) and not isinstance(member.type, BoundedSequence)]@ - cdr << ros_message.@(member.name); -@[ else]@ - cdr << static_cast(size); -@[ if isinstance(member.type.value_type, BasicType) and member.type.value_type.typename not in ('boolean', 'wchar')]@ - if (size > 0) { - cdr.serializeArray(&(ros_message.@(member.name)[0]), size); - } -@[ else]@ -@[ if isinstance(member.type.value_type, AbstractWString)]@ - std::wstring wstr; -@[ end if]@ - for (size_t i = 0; i < size; i++) { -@[ if isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'boolean']@ - cdr << (ros_message.@(member.name)[i] ? true : false); -@[ elif isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'wchar']@ - cdr << static_cast(ros_message.@(member.name)[i]); -@[ elif isinstance(member.type.value_type, AbstractWString)]@ - rosidl_typesupport_fastrtps_cpp::u16string_to_wstring(ros_message.@(member.name)[i], wstr); - cdr << wstr; -@[ elif not isinstance(member.type.value_type, NamespacedType)]@ - cdr << ros_message.@(member.name)[i]; -@[ else]@ - @('::'.join(member.type.value_type.namespaces))::typesupport_fastrtps_cpp::cdr_serialize( - ros_message.@(member.name)[i], - cdr); -@[ end if]@ - } -@[ end if]@ -@[ end if]@ -@[ end if]@ - } -@[ elif isinstance(member.type, BasicType) and member.type.typename == 'boolean']@ - cdr << (ros_message.@(member.name) ? true : false); -@[ elif isinstance(member.type, BasicType) and member.type.typename == 'wchar']@ - cdr << static_cast(ros_message.@(member.name)); -@[ elif isinstance(member.type, AbstractWString)]@ - { - std::wstring wstr; - rosidl_typesupport_fastrtps_cpp::u16string_to_wstring(ros_message.@(member.name), wstr); - cdr << wstr; - } -@[ elif not isinstance(member.type, NamespacedType)]@ - cdr << ros_message.@(member.name); -@[ else]@ - @('::'.join(member.type.namespaces))::typesupport_fastrtps_cpp::cdr_serialize( - ros_message.@(member.name), - cdr); -@[ end if]@ +@[ for line in generate_member_for_cdr_serialize(member, '')]@ + @(line) +@[ end for]@ + @[end for]@ return true; } @@ -289,6 +316,78 @@ cdr_deserialize( return true; } +@{ + +# Generates the definition for the get_serialized_size 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 + +def generate_member_for_get_serialized_size(member, suffix): + 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 + from rosidl_parser.definition import AbstractNestedType + from rosidl_parser.definition import AbstractSequence + 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 + strlist = [] + strlist.append('// Member: %s' % (member.name)) + + if isinstance(member.type, AbstractNestedType): + strlist.append('{') + if isinstance(member.type, Array): + strlist.append(' size_t array_size = %d;' % (member.type.size)) + else: + strlist.append(' size_t array_size = ros_message.%s.size();' % (member.name)) + if isinstance(member.type, BoundedSequence): + strlist.append(' if (array_size > %d) {' % (member.type.maximum_size)) + strlist.append(' throw std::runtime_error("array size exceeds upper bound");') + strlist.append(' }') + strlist.append(' current_alignment += padding +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, padding);') + if isinstance(member.type.value_type, AbstractGenericString): + strlist.append(' for (size_t index = 0; index < array_size; ++index) {') + strlist.append(' current_alignment += padding +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, padding) +') + if isinstance(member.type.value_type, AbstractWString): + strlist.append(' wchar_size *') + strlist.append(' (ros_message.%s[index].size() + 1);' % (member.name)) + strlist.append(' }') + elif isinstance(member.type.value_type, BasicType): + strlist.append(' size_t item_size = sizeof(ros_message.%s[0]);' % (member.name)) + strlist.append(' current_alignment += array_size * item_size +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, item_size);') + else: + strlist.append(' for (size_t index = 0; index < array_size; ++index) {') + strlist.append(' current_alignment +=') + strlist.append(' %s::typesupport_fastrtps_cpp::get_serialized_size%s(' % (('::'.join(member.type.value_type.namespaces)), suffix)) + strlist.append(' ros_message.%s[index], current_alignment);' % (member.name)) + strlist.append(' }') + strlist.append('}') + else: + if isinstance(member.type, AbstractGenericString): + strlist.append('current_alignment += padding +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, padding) +') + if isinstance(member.type, AbstractWString): + strlist.append(' wchar_size *') + strlist.append(' (ros_message.%s.size() + 1);' % (member.name)) + elif isinstance(member.type, BasicType): + strlist.append('{') + strlist.append(' size_t item_size = sizeof(ros_message.%s);' % (member.name)) + strlist.append(' current_alignment += item_size +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, item_size);') + strlist.append('}') + else: + strlist.append('current_alignment +=') + strlist.append(' %s::typesupport_fastrtps_cpp::get_serialized_size%s(' % (('::'.join(member.type.namespaces)), suffix)) + strlist.append(' ros_message.%s, current_alignment);' % (member.name)) + + return strlist; +}@ + size_t ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) get_serialized_size( @@ -303,68 +402,105 @@ get_serialized_size( (void)wchar_size; @[for member in message.structure.members]@ - // Member: @(member.name) -@[ if isinstance(member.type, AbstractNestedType)]@ - { -@[ if isinstance(member.type, Array)]@ - size_t array_size = @(member.type.size); -@[ else]@ - size_t array_size = ros_message.@(member.name).size(); -@[ if isinstance(member.type, BoundedSequence)]@ - if (array_size > @(member.type.maximum_size)) { - throw std::runtime_error("array size exceeds upper bound"); - } -@[ end if]@ +@[ for line in generate_member_for_get_serialized_size(member, '')]@ + @(line) +@[ end for]@ - current_alignment += padding + - eprosima::fastcdr::Cdr::alignment(current_alignment, padding); -@[ 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); - } -@[ elif isinstance(member.type.value_type, BasicType)]@ - size_t item_size = sizeof(ros_message.@(member.name)[0]); - current_alignment += array_size * item_size + - eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); -@[ else] - for (size_t index = 0; index < array_size; ++index) { - current_alignment += - @('::'.join(member.type.value_type.namespaces))::typesupport_fastrtps_cpp::get_serialized_size( - ros_message.@(member.name)[index], current_alignment); - } -@[ end if]@ - } -@[ 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); -@[ elif isinstance(member.type, BasicType)]@ - { - size_t item_size = sizeof(ros_message.@(member.name)); - current_alignment += item_size + - eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); - } -@[ else] - current_alignment += - @('::'.join(member.type.namespaces))::typesupport_fastrtps_cpp::get_serialized_size( - ros_message.@(member.name), current_alignment); -@[ end if]@ -@[ end if]@ @[end for]@ - return current_alignment - initial_alignment; } +@{ + +# Generates the definition for the max_serialized_size 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 + +def generate_member_for_max_serialized_size(member, suffix): + 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 + from rosidl_parser.definition import AbstractNestedType + from rosidl_parser.definition import AbstractSequence + 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 + strlist = [] + strlist.append('// Member: %s' % (member.name)) + strlist.append('{') + + if isinstance(member.type, AbstractNestedType): + if isinstance(member.type, Array): + strlist.append(' size_t array_size = %d;' % (member.type.size)) + elif isinstance(member.type, BoundedSequence): + strlist.append(' size_t array_size = %d;' % (member.type.maximum_size)) + else: + strlist.append(' size_t array_size = 0;') + strlist.append(' full_bounded = false;') + if isinstance(member.type, AbstractSequence): + strlist.append(' is_plain = false;') + strlist.append(' current_alignment += padding +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, padding);') + else: + strlist.append(' size_t array_size = 1;') + + type_ = member.type + if isinstance(type_, AbstractNestedType): + type_ = type_.value_type + + if isinstance(type_, AbstractGenericString): + strlist.append(' full_bounded = false;') + strlist.append(' is_plain = false;') + strlist.append(' for (size_t index = 0; index < array_size; ++index) {') + strlist.append(' current_alignment += padding +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + ') + if type_.has_maximum_size(): + if isinstance(type_, AbstractWString): + strlist.append(' wchar_size *') + strlist.append(' %d +' % (type_.maximum_size)) + if isinstance(type_, AbstractWString): + strlist.append(' wchar_size *') + strlist.append(' 1;') + strlist.append(' }') + elif isinstance(type_, BasicType): + if type_.typename in ('boolean', 'octet', 'char', 'uint8', 'int8'): + strlist.append(' last_member_size = array_size * sizeof(uint8_t);') + strlist.append(' current_alignment += array_size * sizeof(uint8_t);') + elif type_.typename in ('wchar', 'int16', 'uint16'): + strlist.append(' last_member_size = array_size * sizeof(uint16_t);') + strlist.append(' current_alignment += array_size * sizeof(uint16_t) +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint16_t));') + elif type_.typename in ('int32', 'uint32', 'float'): + strlist.append(' last_member_size = array_size * sizeof(uint32_t);') + strlist.append(' current_alignment += array_size * sizeof(uint32_t) +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t));') + elif type_.typename in ('int64', 'uint64', 'double'): + strlist.append(' last_member_size = array_size * sizeof(uint64_t);') + strlist.append(' current_alignment += array_size * sizeof(uint64_t) +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t));') + elif type_.typename == 'long double': + strlist.append(' last_member_size = array_size * sizeof(long double);') + strlist.append(' current_alignment += array_size * sizeof(long double) +') + strlist.append(' eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(long double));') + else: + strlist.append(' last_member_size = 0;') + strlist.append(' for (size_t index = 0; index < array_size; ++index) {') + strlist.append(' bool inner_full_bounded;') + strlist.append(' bool inner_is_plain;') + strlist.append(' size_t inner_size =') + strlist.append(' %s::typesupport_fastrtps_cpp::max_serialized_size%s_%s(' % (('::'.join(type_.namespaces)), suffix, type_.name)) + strlist.append(' inner_full_bounded, inner_is_plain, current_alignment);') + strlist.append(' last_member_size += inner_size;') + strlist.append(' current_alignment += inner_size;') + strlist.append(' full_bounded &= inner_full_bounded;') + strlist.append(' is_plain &= inner_is_plain;') + strlist.append(' }') + strlist.append('}') + return strlist +}@ + size_t ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) max_serialized_size_@(message.structure.namespaced_type.name)( @@ -391,87 +527,118 @@ last_member_name_ = None @{ last_member_name_ = member.name }@ +@[ for line in generate_member_for_max_serialized_size(member, '')]@ + @(line) +@[ end for]@ - // Member: @(member.name) - { -@[ if isinstance(member.type, AbstractNestedType)]@ -@[ if isinstance(member.type, Array)]@ - size_t array_size = @(member.type.size); -@[ elif isinstance(member.type, BoundedSequence)]@ - size_t array_size = @(member.type.maximum_size); -@[ else]@ - size_t array_size = 0; - full_bounded = false; -@[ end if]@ -@[ if isinstance(member.type, AbstractSequence)]@ - is_plain = false; - current_alignment += padding + - eprosima::fastcdr::Cdr::alignment(current_alignment, padding); -@[ end if]@ -@[ else]@ - size_t array_size = 1; +@[end for]@ + + size_t ret_val = current_alignment - initial_alignment; +@[if last_member_name_ is not None]@ + if (is_plain) { + // All members are plain, and type is not empty. + // We still need to check that the in-memory alignment + // is the same as the CDR mandated alignment. + using DataType = @('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])); + is_plain = + ( + offsetof(DataType, @(last_member_name_)) + + last_member_size + ) == ret_val; + } +@[end if]@ + return ret_val; +} + +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +cdr_serialize_key( + const @('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])) & ros_message, + eprosima::fastcdr::Cdr & cdr) +{ +@[for member in message.structure.members]@ +@[ if not member.has_annotation('key') and message.structure.has_any_member_with_annotation('key')]@ +@[ continue]@ @[ end if]@ +@[ for line in generate_member_for_cdr_serialize(member, '_key')]@ + @(line) +@[ end for]@ + +@[end for]@ + return true; +} + +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +cdr_deserialize_key( + const @('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])) & ros_message, + eprosima::fastcdr::Cdr & cdr) +{ + (void)ros_message; + (void)cdr; + // TODO + return false; +} + +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +get_serialized_size_key( + const @('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])) & ros_message, + size_t current_alignment) +{ + 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]@ +@[ if not member.has_annotation('key') and message.structure.has_any_member_with_annotation('key')]@ +@[ continue]@ +@[ end if]@ +@[ for line in generate_member_for_get_serialized_size(member, '_key')]@ + @(line) +@[ end for]@ + +@[end for]@ + return current_alignment - initial_alignment; +} + +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +max_serialized_size_key_@(message.structure.namespaced_type.name)( + bool & full_bounded, + bool & is_plain, + size_t current_alignment) +{ + size_t initial_alignment = current_alignment; + + const size_t padding = 4; + const size_t wchar_size = 4; + size_t last_member_size = 0; + (void)last_member_size; + (void)padding; + (void)wchar_size; + + full_bounded = true; + is_plain = true; @{ -type_ = member.type -if isinstance(type_, AbstractNestedType): - type_ = type_.value_type +last_member_name_ = None }@ -@[ 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; - } -@[ elif isinstance(type_, BasicType)]@ -@[ if type_.typename in ('boolean', 'octet', 'char', 'uint8', 'int8')]@ - last_member_size = array_size * sizeof(uint8_t); - current_alignment += array_size * sizeof(uint8_t); -@[ elif type_.typename in ('wchar', 'int16', '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)); -@[ elif type_.typename in ('int32', 'uint32', 'float')]@ - last_member_size = array_size * sizeof(uint32_t); - current_alignment += array_size * sizeof(uint32_t) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t)); -@[ elif type_.typename in ('int64', 'uint64', 'double')]@ - last_member_size = array_size * sizeof(uint64_t); - current_alignment += array_size * sizeof(uint64_t) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t)); -@[ elif type_.typename == 'long double']@ - last_member_size = array_size * sizeof(long double); - current_alignment += array_size * sizeof(long double) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(long double)); -@[ end if]@ -@[ else] - last_member_size = 0; - for (size_t index = 0; index < array_size; ++index) { - bool inner_full_bounded; - bool inner_is_plain; - size_t inner_size = - @('::'.join(type_.namespaces))::typesupport_fastrtps_cpp::max_serialized_size_@(type_.name)( - inner_full_bounded, inner_is_plain, current_alignment); - last_member_size += inner_size; - current_alignment += inner_size; - full_bounded &= inner_full_bounded; - is_plain &= inner_is_plain; - } +@[for member in message.structure.members]@ +@{ +last_member_name_ = member.name +}@ +@[ if not member.has_annotation('key') and message.structure.has_any_member_with_annotation('key')]@ +@[ continue]@ @[ end if]@ - } -@[end for]@ +@[ for line in generate_member_for_max_serialized_size(member, '_key')]@ + @(line) +@[ end for]@ +@[end for]@ size_t ret_val = current_alignment - initial_alignment; @[if last_member_name_ is not None]@ if (is_plain) { @@ -485,11 +652,70 @@ if isinstance(type_, AbstractNestedType): last_member_size ) == ret_val; } - @[end if]@ return ret_val; } +@[ if message.structure.has_any_member_with_annotation('key') ]@ +static bool _@(message.structure.namespaced_type.name)__cdr_serialize_key( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr) +{ + auto typed_message = + static_cast( + untyped_ros_message); + + return cdr_serialize_key(*typed_message, cdr); +} + +static +bool +_@(message.structure.namespaced_type.name)__cdr_deserialize_key( + eprosima::fastcdr::Cdr & cdr, + void * untyped_ros_message) +{ + 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_key(cdr, *typed_message); +} + +static +size_t +_@(message.structure.namespaced_type.name)__get_serialized_size_key( + const void * untyped_ros_message) +{ + auto typed_message = + static_cast( + untyped_ros_message); + + return get_serialized_size_key(*typed_message, 0); +} + +static size_t _@(message.structure.namespaced_type.name)__max_serialized_size_key( + bool & is_unbounded) +{ + bool full_bounded = true; + bool is_plain = true; + + size_t ret_val = max_serialized_size_key_@(message.structure.namespaced_type.name)( + full_bounded, + is_plain, + 0); + + is_unbounded = !full_bounded; + return ret_val; +} + +static message_type_support_key_callbacks_t _@(message.structure.namespaced_type.name)__key_callbacks = { + _@(message.structure.namespaced_type.name)__cdr_serialize_key, + _@(message.structure.namespaced_type.name)__cdr_deserialize_key, + _@(message.structure.namespaced_type.name)__get_serialized_size_key, + _@(message.structure.namespaced_type.name)__max_serialized_size_key, +}; +@[ end if]@ + static bool _@(message.structure.namespaced_type.name)__cdr_serialize( const void * untyped_ros_message, eprosima::fastcdr::Cdr & cdr) @@ -539,11 +765,16 @@ static message_type_support_callbacks_t _@(message.structure.namespaced_type.nam _@(message.structure.namespaced_type.name)__cdr_serialize, _@(message.structure.namespaced_type.name)__cdr_deserialize, _@(message.structure.namespaced_type.name)__get_serialized_size, - _@(message.structure.namespaced_type.name)__max_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 +@[ else]@ + nullptr +@[ end if]@ }; static rosidl_message_type_support_t _@(message.structure.namespaced_type.name)__handle = { - rosidl_typesupport_fastrtps_cpp::typesupport_identifier, + rosidl_typesupport_fastrtps_cpp::typesupport_identifier_v2, &_@(message.structure.namespaced_type.name)__callbacks, get_message_typesupport_handle_function, &@(idl_structure_type_to_c_typename(message.structure.namespaced_type))__@(GET_HASH_FUNC), diff --git a/rosidl_typesupport_fastrtps_cpp/src/identifier.cpp b/rosidl_typesupport_fastrtps_cpp/src/identifier.cpp index 8c991d5..11b7d26 100644 --- a/rosidl_typesupport_fastrtps_cpp/src/identifier.cpp +++ b/rosidl_typesupport_fastrtps_cpp/src/identifier.cpp @@ -20,4 +20,7 @@ namespace rosidl_typesupport_fastrtps_cpp ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT const char * typesupport_identifier = "rosidl_typesupport_fastrtps_cpp"; +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT +const char * typesupport_identifier_v2 = "rosidl_typesupport_fastrtps_cpp_v2"; + } // namespace rosidl_typesupport_fastrtps_cpp