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..224ced2 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,22 @@ 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) +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 1c29b6a..ef90076 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 @@ -137,6 +137,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]@ @@ -152,6 +167,28 @@ 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]@ +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]@ @@ -163,131 +200,129 @@ 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(' 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::cdr_serialize(cdr, *str);') + 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.serialize_array(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(' rosidl_typesupport_fastrtps_c::cdr_serialize(cdr, ros_message->%s);' % (member.name)) + 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)]@ - 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::cdr_serialize(cdr, *str); - } -@[ 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.serialize_array(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)]@ - rosidl_typesupport_fastrtps_c::cdr_serialize(cdr, ros_message->@(member.name)); -@[ 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) { @@ -296,13 +331,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); @@ -382,11 +410,7 @@ else: cdr.deserialize_array(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)]@ @@ -423,11 +447,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]@ } @@ -435,6 +455,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, @@ -450,69 +538,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]))( @@ -539,87 +664,105 @@ 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) +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) { @@ -633,11 +776,90 @@ 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 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)__max_serialized_size_key, + _@(message.structure.namespaced_type.name)__get_serialized_size_key, + _@(message.structure.namespaced_type.name)__cdr_serialize_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; @@ -662,7 +884,12 @@ static message_type_support_callbacks_t __callbacks_@(message.structure.namespac _@(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 = {