From 1140912ec92311431eb87888d7179dbbe62d7353 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Tue, 19 Dec 2023 15:24:42 +0000 Subject: [PATCH 01/25] Refs #20156: Initial infraestructure for get_key_type_support() api method Signed-off-by: Mario Dominguez --- .../resource/msg__type_support_c.cpp.em | 20 +++++- .../message_type_support.h | 62 +++++++++++++++++++ ...sg__rosidl_typesupport_fastrtps_cpp.hpp.em | 7 +++ .../resource/msg__type_support.cpp.em | 23 ++++++- 4 files changed, 110 insertions(+), 2 deletions(-) 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..5d6d4b1 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 @@ -644,6 +644,16 @@ if isinstance(type_, AbstractNestedType): return ret_val; } +bool +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +get_key_type_support_@(message.structure.namespaced_type.name)( + message_type_support_key_callbacks_t * key_callbacks) +{ + //TODO + static_cast(key_callbacks); + return false; +} + static size_t _@(message.structure.namespaced_type.name)__max_serialized_size(char & bounds_info) { bool full_bounded; @@ -659,6 +669,13 @@ static size_t _@(message.structure.namespaced_type.name)__max_serialized_size(ch return ret_val; } +static bool _@(message.structure.namespaced_type.name)__get_key_type_support(message_type_support_key_callbacks_t * key_callbacks) +{ + bool ret_val; + ret_val = get_key_type_support_@(message.structure.namespaced_type.name)(key_callbacks); + return ret_val; +} + @ @# // Collect the callback functions and provide a function to get the type support struct. @@ -668,7 +685,8 @@ 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, + _@(message.structure.namespaced_type.name)__get_key_type_support }; static rosidl_message_type_support_t _@(message.structure.namespaced_type.name)__type_support = { 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..9bc54ae 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,58 @@ #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 message 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 message 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 for retrieving the key hash + /** + * \param [in] capacity The desired hash size that tha buffer is meant to have allocated. + * \param [in] force_md5 Whether to force md5 checksum. + * \param [in, out] length The number of bytes written in the buffer. + * \param [in, out] buffer The buffer where to store the hash. + */ + void (* get_key_hash)( + size_t capacity, + bool force_md5, + size_t* length, + uint8_t* buffer); + + /// Callback function to get size of the key data + /** + * \return The size of the serialized message in bytes. + */ + uint32_t (* get_serialized_key_size)( + const void * untyped_ros_message); + + /// Callback function to determine the maximum size needed for key serialization, + /// which is used for key type support initialization. + /** + * \return The maximum key serialized size, in bytes. + */ + size_t (* max_serialized_key_size)(); + +} 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 +128,16 @@ typedef struct message_type_support_callbacks_t * \return The maximum serialized size, in bytes. */ size_t (* max_serialized_size)(char & bounds_info); + + /// Checks whether the type has keys, filling the incoming argument + /// with the corresponding callbacks when true. + /** + * \param [in,out] key_callbacks Associated struct of key callbacks. + * It is filled only if the type is keyed. + * \return True if the type has a key. + */ + bool (* get_key_type_support)(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..e96a57a 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,8 @@ header_files = [ @[ end if]@ #include "@(header_file)" @[end for]@ + +struct message_type_support_key_callbacks_t; @[for ns in message.structure.namespaced_type.namespaces]@ namespace @(ns) @@ -83,6 +85,11 @@ max_serialized_size_@(message.structure.namespaced_type.name)( bool & is_plain, size_t current_alignment); +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +get_key_type_support_@(message.structure.namespaced_type.name)( + message_type_support_key_callbacks_t * key_callbacks); + } // 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..20fb018 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,9 @@ max_serialized_size_@(type_.name)( bool & full_bounded, bool & is_plain, size_t current_alignment); +bool +get_key_type_support_@(type_.name)( + message_type_support_key_callbacks_t * key_callbacks); } // namespace typesupport_fastrtps_cpp @[ for ns in reversed(type_.namespaces)]@ } // namespace @(ns) @@ -490,6 +493,16 @@ if isinstance(type_, AbstractNestedType): return ret_val; } +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +get_key_type_support_@(message.structure.namespaced_type.name)( + message_type_support_key_callbacks_t * key_callbacks) +{ + //TODO + static_cast(key_callbacks); + return false; +} + static bool _@(message.structure.namespaced_type.name)__cdr_serialize( const void * untyped_ros_message, eprosima::fastcdr::Cdr & cdr) @@ -533,13 +546,21 @@ static size_t _@(message.structure.namespaced_type.name)__max_serialized_size(ch return ret_val; } +static bool _@(message.structure.namespaced_type.name)__get_key_type_support( message_type_support_key_callbacks_t * key_callbacks) +{ + bool ret_val; + ret_val = get_key_type_support_@(message.structure.namespaced_type.name)(key_callbacks); + return ret_val; +} + static message_type_support_callbacks_t _@(message.structure.namespaced_type.name)__callbacks = { "@('::'.join([package_name] + list(interface_path.parents[0].parts)))", "@(message.structure.namespaced_type.name)", _@(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, + _@(message.structure.namespaced_type.name)__get_key_type_support }; static rosidl_message_type_support_t _@(message.structure.namespaced_type.name)__handle = { From ae41c591153e441681e3fceba9007ad1868c2fda Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Wed, 20 Dec 2023 12:29:59 +0000 Subject: [PATCH 02/25] Refs #20156: Return either true or false depending if the msg has keys Signed-off-by: Mario Dominguez --- .../resource/msg__type_support_c.cpp.em | 11 ++++++++--- .../resource/msg__type_support.cpp.em | 11 ++++++++--- 2 files changed, 16 insertions(+), 6 deletions(-) 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 5d6d4b1..4a16326 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 @@ -649,9 +649,14 @@ ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) get_key_type_support_@(message.structure.namespaced_type.name)( message_type_support_key_callbacks_t * key_callbacks) { - //TODO - static_cast(key_callbacks); - return false; +@[ if message.structure.has_any_member_with_annotation('key') ]@ + //TODO + static_cast(key_callbacks); + return true; +@[ else]@ + static_cast(key_callbacks); + return false; +@[ end if]@ } static size_t _@(message.structure.namespaced_type.name)__max_serialized_size(char & bounds_info) 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 20fb018..be986d3 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -498,9 +498,14 @@ ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) get_key_type_support_@(message.structure.namespaced_type.name)( message_type_support_key_callbacks_t * key_callbacks) { - //TODO - static_cast(key_callbacks); - return false; +@[ if message.structure.has_any_member_with_annotation('key') ]@ + //TODO + static_cast(key_callbacks); + return true; +@[ else]@ + static_cast(key_callbacks); + return false; +@[ end if]@ } static bool _@(message.structure.namespaced_type.name)__cdr_serialize( From ba1542bcda604373d11295a50040caa7aa582d89 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Tue, 30 Jan 2024 11:36:56 +0000 Subject: [PATCH 03/25] Refs #20156: Update message_type_support_key_callbacks_t structure Signed-off-by: Mario Dominguez --- .../message_type_support.h | 24 +++++++------------ 1 file changed, 8 insertions(+), 16 deletions(-) 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 9bc54ae..44d6065 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 @@ -49,32 +49,24 @@ typedef struct message_type_support_key_callbacks_t eprosima::fastcdr::Cdr & cdr, void * untyped_ros_message); - /// Callback function for retrieving the key hash - /** - * \param [in] capacity The desired hash size that tha buffer is meant to have allocated. - * \param [in] force_md5 Whether to force md5 checksum. - * \param [in, out] length The number of bytes written in the buffer. - * \param [in, out] buffer The buffer where to store the hash. - */ - void (* get_key_hash)( - size_t capacity, - bool force_md5, - size_t* length, - uint8_t* buffer); - /// Callback function to get size of the key data /** * \return The size of the serialized message in bytes. */ - uint32_t (* get_serialized_key_size)( - const void * untyped_ros_message); + size_t (* get_serialized_key_size)( + const void * untyped_ros_message, + size_t initial_alignment); /// Callback function to determine the maximum size needed for key serialization, /// which is used for key type support initialization. /** + * \param [in] initial_alignment Initial alignment to be incrementally calculated. + * \param [in,out] is_unbounded Whether the key has any unbounded member. * \return The maximum key serialized size, in bytes. */ - size_t (* max_serialized_key_size)(); + size_t (* max_serialized_key_size)( + size_t initial_aligment, + bool & is_unbounded); } message_type_support_key_callbacks_t; From c95f3990be4dcf649cbebdf69873542cabe11217 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Tue, 30 Jan 2024 11:37:59 +0000 Subject: [PATCH 04/25] Refs #20156: Implement rosidl_typesupport_fastrtps_c generator methods for supporting keys Signed-off-by: Mario Dominguez --- .../resource/msg__type_support_c.cpp.em | 333 +++++++++++++++++- 1 file changed, 328 insertions(+), 5 deletions(-) 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 4a16326..8d7ff42 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 @@ -644,18 +644,341 @@ if isinstance(type_, AbstractNestedType): return ret_val; } +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +_@(message.structure.namespaced_type.name)__max_serialized_key_size( + size_t initial_alignment, + bool & is_unbounded) +{ +@[ if message.structure.has_any_member_with_annotation('key') ]@ + size_t current_alignment = initial_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')]@ +@[ continue]@ +@[ end if]@ + // 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; +@[ end if]@ +@[ if isinstance(member.type, AbstractSequence)]@ + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding); +@[ end if]@ +@[ else]@ + size_t array_size = 1; +@[ end if]@ + +@{ +type_ = member.type +if isinstance(type_, AbstractNestedType): + type_ = type_.value_type +}@ +@[ if isinstance(type_, AbstractGenericString)]@ + is_unbounded = true; + 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')]@ + current_alignment += array_size * sizeof(uint8_t); +@[ elif type_.typename in ('wchar', 'int16', 'uint16')]@ + current_alignment += array_size * sizeof(uint16_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint16_t)); +@[ elif type_.typename in ('int32', 'uint32', 'float')]@ + current_alignment += array_size * sizeof(uint32_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t)); +@[ elif type_.typename in ('int64', 'uint64', 'double')]@ + current_alignment += array_size * sizeof(uint64_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t)); +@[ elif type_.typename == 'long double']@ + current_alignment += array_size * sizeof(long double) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(long double)); +@[ end if]@ +@[ else] + for (size_t index = 0; index < array_size; ++index) { + current_alignment += + _@(message.structure.namespaced_type.name)__max_serialized_key_size( + current_alignment, is_unbounded); + } +@[ end if]@ + } +@[end for]@ + + return current_alignment - initial_alignment; +@[ else]@ + static_cast(initial_alignment); + static_cast(is_unbounded); + return static_cast(0); +@[ end if]@ +} + +bool +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +_@(message.structure.namespaced_type.name)__cdr_deserialize_key( + eprosima::fastcdr::Cdr &, + void *) +{ + return false; +} + +bool +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +_@(message.structure.namespaced_type.name)__cdr_serialize_key( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr) +{ +@[ if message.structure.has_any_member_with_annotation('key') ]@ +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]@ +@[ if not member.has_annotation('key')]@ +@[ continue]@ +@[ end if]@ + // Field name: @(member.name) + { +@{ +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); + 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]@ + } + +@[end for]@ + return true; +@[ else]@ + static_cast(untyped_ros_message); + static_cast(cdr); + return false; +@[ end if]@ +} + +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) +_@(message.structure.namespaced_type.name)__get_serialized_key_size( + const void * untyped_ros_message, + size_t initial_alignment) +{ +@[ if message.structure.has_any_member_with_annotation('key') ]@ + auto ros_message = + static_cast( + untyped_ros_message); + + size_t current_alignment = initial_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')]@ +@[ continue]@ +@[ end if]@ + // 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]@ + + 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 += + _@(message.structure.namespaced_type.name)__get_serialized_key_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 += + _@(message.structure.namespaced_type.name)__get_serialized_key_size( + ros_message->@(member.name), current_alignment); +@[ end if]@ +@[ end if]@ +@[end for]@ + + return current_alignment - initial_alignment; +@[ else]@ + static_cast(untyped_ros_message); + static_cast(initial_alignment); + return static_cast(0); +@[ end if]@ +} + bool ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) get_key_type_support_@(message.structure.namespaced_type.name)( message_type_support_key_callbacks_t * key_callbacks) { + std::cout << "Initializing C interface @(message.structure.namespaced_type.name)" << std::endl; @[ if message.structure.has_any_member_with_annotation('key') ]@ - //TODO - static_cast(key_callbacks); - return true; + key_callbacks->max_serialized_key_size = &_@(message.structure.namespaced_type.name)__max_serialized_key_size; + key_callbacks->cdr_deserialize_key = &_@(message.structure.namespaced_type.name)__cdr_deserialize_key; + key_callbacks->cdr_serialize_key = &_@(message.structure.namespaced_type.name)__cdr_serialize_key; + key_callbacks->get_serialized_key_size = &_@(message.structure.namespaced_type.name)__get_serialized_key_size; + return true; @[ else]@ - static_cast(key_callbacks); - return false; + static_cast(key_callbacks); + return false; @[ end if]@ } From 6452b9c27230ea4478352e04bc28a08abd7a2931 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Tue, 30 Jan 2024 11:38:08 +0000 Subject: [PATCH 05/25] Refs #20156: Implement rosidl_typesupport_fastrtps_cpp generator methods for supporting keys Signed-off-by: Mario Dominguez --- .../resource/msg__type_support.cpp.em | 304 +++++++++++++++++- 1 file changed, 299 insertions(+), 5 deletions(-) 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 be986d3..2bb19bf 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -493,18 +493,312 @@ if isinstance(type_, AbstractNestedType): return ret_val; } +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +_@(message.structure.namespaced_type.name)__max_serialized_key_size( + size_t initial_alignment, + bool & is_unbounded) +{ +@[ if message.structure.has_any_member_with_annotation('key') ]@ + size_t current_alignment = initial_alignment; +@[for member in message.structure.members]@ +@[ if not member.has_annotation('key')]@ +@[ continue]@ +@[ end if]@ + // 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; +@[ end if]@ +@[ if isinstance(member.type, AbstractSequence)]@ + const size_t padding = 4; + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding); +@[ end if]@ +@[ else]@ + size_t array_size = 1; +@[ end if]@ + +@{ +type_ = member.type +if isinstance(type_, AbstractNestedType): + type_ = type_.value_type +}@ +@[ if isinstance(type_, AbstractGenericString)]@ + is_unbounded = true; + const size_t padding = 4; + const size_t wchar_size = 4; + + 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')]@ + current_alignment += array_size * sizeof(uint8_t); +@[ elif type_.typename in ('wchar', 'int16', 'uint16')]@ + current_alignment += array_size * sizeof(uint16_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint16_t)); +@[ elif type_.typename in ('int32', 'uint32', 'float')]@ + current_alignment += array_size * sizeof(uint32_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t)); +@[ elif type_.typename in ('int64', 'uint64', 'double')]@ + current_alignment += array_size * sizeof(uint64_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t)); +@[ elif type_.typename == 'long double']@ + current_alignment += array_size * sizeof(long double) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(long double)); +@[ end if]@ +@[ else] + for (size_t index = 0; index < array_size; ++index) { + current_alignment += + @('::'.join(type_.namespaces))::typesupport_fastrtps_cpp::_@(message.structure.namespaced_type.name)__max_serialized_key_size( + current_alignment, is_unbounded); + } +@[ end if]@ + } +@[end for]@ + + return current_alignment - initial_alignment; +@[ else]@ + static_cast(initial_alignment); + static_cast(is_unbounded); + return static_cast(0); +@[ end if]@ +} + +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +_@(message.structure.namespaced_type.name)__cdr_deserialize_key( + eprosima::fastcdr::Cdr &, + void *) +{ + return false; +} + +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +_@(message.structure.namespaced_type.name)__cdr_serialize_key( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr) +{ + +@[ if message.structure.has_any_member_with_annotation('key') ]@ + auto ros_message = + static_cast( + untyped_ros_message); + +@[for member in message.structure.members]@ +@[ if not member.has_annotation('key')]@ +@[ continue]@ +@[ end if]@ + // 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("key 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]@ +@[end for]@ + return true; +@[ else]@ + static_cast(untyped_ros_message); + static_cast(cdr); + return false; +@[ end if]@ +} + +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) +_@(message.structure.namespaced_type.name)__get_serialized_key_size( + const void * untyped_ros_message, + size_t initial_alignment) +{ +@[ if message.structure.has_any_member_with_annotation('key') ]@ + auto ros_message = + static_cast( + untyped_ros_message); + + size_t current_alignment = initial_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')]@ +@[ continue]@ +@[ end if]@ + // 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]@ + + 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::_@(message.structure.namespaced_type.name)__get_serialized_key_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::_@(message.structure.namespaced_type.name)__get_serialized_key_size( + ros_message->@(member.name), current_alignment); +@[ end if]@ +@[ end if]@ +@[end for]@ + + return current_alignment - initial_alignment; +@[ else]@ + static_cast(untyped_ros_message); + static_cast(initial_alignment); + return static_cast(0); +@[ end if]@ +} + bool ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) get_key_type_support_@(message.structure.namespaced_type.name)( message_type_support_key_callbacks_t * key_callbacks) { + std::cout << "Initializing CPP interface @(message.structure.namespaced_type.name)" << std::endl; @[ if message.structure.has_any_member_with_annotation('key') ]@ - //TODO - static_cast(key_callbacks); - return true; + key_callbacks->max_serialized_key_size = &_@(message.structure.namespaced_type.name)__max_serialized_key_size; + key_callbacks->cdr_deserialize_key = &_@(message.structure.namespaced_type.name)__cdr_deserialize_key; + key_callbacks->cdr_serialize_key = &_@(message.structure.namespaced_type.name)__cdr_serialize_key; + key_callbacks->get_serialized_key_size = &_@(message.structure.namespaced_type.name)__get_serialized_key_size; + return true; @[ else]@ - static_cast(key_callbacks); - return false; + static_cast(key_callbacks); + return false; @[ end if]@ } From 7bd1b9267bd79329841fc3922b88258023cc69b2 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 8 Feb 2024 09:29:07 +0000 Subject: [PATCH 06/25] Refs #20310: Define v2 ABI identifier Signed-off-by: Mario Dominguez --- .../include/rosidl_typesupport_fastrtps_c/identifier.h | 4 ++++ rosidl_typesupport_fastrtps_c/src/identifier.cpp | 3 +++ .../include/rosidl_typesupport_fastrtps_cpp/identifier.hpp | 4 ++++ rosidl_typesupport_fastrtps_cpp/src/identifier.cpp | 3 +++ 4 files changed, 14 insertions(+) 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/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/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 From 955a8ec40b76b83d970f9637000d6215407d65e0 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 8 Feb 2024 09:33:45 +0000 Subject: [PATCH 07/25] Refs #20310: Replace get_key_type_support() with a raw pointer to the structure Signed-off-by: Mario Dominguez --- .../resource/msg__type_support_c.cpp.em | 65 +++++------------- .../message_type_support.h | 11 +--- .../resource/msg__type_support.cpp.em | 66 +++++-------------- 3 files changed, 39 insertions(+), 103 deletions(-) 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 8d7ff42..5b3da8d 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 @@ -644,14 +644,15 @@ if isinstance(type_, AbstractNestedType): return ret_val; } +@[ if message.structure.has_any_member_with_annotation('key') ]@ +static size_t -ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) _@(message.structure.namespaced_type.name)__max_serialized_key_size( size_t initial_alignment, bool & is_unbounded) { -@[ if message.structure.has_any_member_with_annotation('key') ]@ size_t current_alignment = initial_alignment; + is_unbounded = false; const size_t padding = 4; const size_t wchar_size = 4; @@ -728,15 +729,10 @@ if isinstance(type_, AbstractNestedType): @[end for]@ return current_alignment - initial_alignment; -@[ else]@ - static_cast(initial_alignment); - static_cast(is_unbounded); - return static_cast(0); -@[ end if]@ } +static bool -ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) _@(message.structure.namespaced_type.name)__cdr_deserialize_key( eprosima::fastcdr::Cdr &, void *) @@ -744,13 +740,12 @@ _@(message.structure.namespaced_type.name)__cdr_deserialize_key( return false; } +static bool -ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) _@(message.structure.namespaced_type.name)__cdr_serialize_key( const void * untyped_ros_message, eprosima::fastcdr::Cdr & cdr) { -@[ if message.structure.has_any_member_with_annotation('key') ]@ if (!untyped_ros_message) { fprintf(stderr, "ros message handle is null\n"); return false; @@ -868,20 +863,14 @@ if isinstance(type_, AbstractNestedType): @[end for]@ return true; -@[ else]@ - static_cast(untyped_ros_message); - static_cast(cdr); - return false; -@[ end if]@ } +static size_t -ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) _@(message.structure.namespaced_type.name)__get_serialized_key_size( const void * untyped_ros_message, size_t initial_alignment) { -@[ if message.structure.has_any_member_with_annotation('key') ]@ auto ros_message = static_cast( untyped_ros_message); @@ -957,30 +946,15 @@ _@(message.structure.namespaced_type.name)__get_serialized_key_size( @[end for]@ return current_alignment - initial_alignment; -@[ else]@ - static_cast(untyped_ros_message); - static_cast(initial_alignment); - return static_cast(0); -@[ end if]@ } -bool -ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(package_name) -get_key_type_support_@(message.structure.namespaced_type.name)( - message_type_support_key_callbacks_t * key_callbacks) -{ - std::cout << "Initializing C interface @(message.structure.namespaced_type.name)" << std::endl; -@[ if message.structure.has_any_member_with_annotation('key') ]@ - key_callbacks->max_serialized_key_size = &_@(message.structure.namespaced_type.name)__max_serialized_key_size; - key_callbacks->cdr_deserialize_key = &_@(message.structure.namespaced_type.name)__cdr_deserialize_key; - key_callbacks->cdr_serialize_key = &_@(message.structure.namespaced_type.name)__cdr_serialize_key; - key_callbacks->get_serialized_key_size = &_@(message.structure.namespaced_type.name)__get_serialized_key_size; - return true; -@[ else]@ - static_cast(key_callbacks); - return false; +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_key_size, + _@(message.structure.namespaced_type.name)__max_serialized_key_size, +}; @[ end if]@ -} static size_t _@(message.structure.namespaced_type.name)__max_serialized_size(char & bounds_info) { @@ -997,13 +971,6 @@ static size_t _@(message.structure.namespaced_type.name)__max_serialized_size(ch return ret_val; } -static bool _@(message.structure.namespaced_type.name)__get_key_type_support(message_type_support_key_callbacks_t * key_callbacks) -{ - bool ret_val; - ret_val = get_key_type_support_@(message.structure.namespaced_type.name)(key_callbacks); - return ret_val; -} - @ @# // Collect the callback functions and provide a function to get the type support struct. @@ -1014,11 +981,15 @@ static message_type_support_callbacks_t __callbacks_@(message.structure.namespac _@(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)__get_key_type_support +@[ 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_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 44d6065..9e81c40 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 @@ -121,14 +121,9 @@ typedef struct message_type_support_callbacks_t */ size_t (* max_serialized_size)(char & bounds_info); - /// Checks whether the type has keys, filling the incoming argument - /// with the corresponding callbacks when true. - /** - * \param [in,out] key_callbacks Associated struct of key callbacks. - * It is filled only if the type is keyed. - * \return True if the type has a key. - */ - bool (* get_key_type_support)(message_type_support_key_callbacks_t * key_callbacks); + /// 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; 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 2bb19bf..28c739f 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -493,14 +493,15 @@ if isinstance(type_, AbstractNestedType): return ret_val; } +@[ if message.structure.has_any_member_with_annotation('key') ]@ +static size_t -ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) _@(message.structure.namespaced_type.name)__max_serialized_key_size( size_t initial_alignment, bool & is_unbounded) { -@[ if message.structure.has_any_member_with_annotation('key') ]@ size_t current_alignment = initial_alignment; + is_unbounded = false; @[for member in message.structure.members]@ @[ if not member.has_annotation('key')]@ @[ continue]@ @@ -575,15 +576,10 @@ if isinstance(type_, AbstractNestedType): @[end for]@ return current_alignment - initial_alignment; -@[ else]@ - static_cast(initial_alignment); - static_cast(is_unbounded); - return static_cast(0); -@[ end if]@ } +static bool -ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) _@(message.structure.namespaced_type.name)__cdr_deserialize_key( eprosima::fastcdr::Cdr &, void *) @@ -591,14 +587,12 @@ _@(message.structure.namespaced_type.name)__cdr_deserialize_key( return false; } +static bool -ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) _@(message.structure.namespaced_type.name)__cdr_serialize_key( const void * untyped_ros_message, eprosima::fastcdr::Cdr & cdr) { - -@[ if message.structure.has_any_member_with_annotation('key') ]@ auto ros_message = static_cast( untyped_ros_message); @@ -688,20 +682,14 @@ _@(message.structure.namespaced_type.name)__cdr_serialize_key( @[ end if]@ @[end for]@ return true; -@[ else]@ - static_cast(untyped_ros_message); - static_cast(cdr); - return false; -@[ end if]@ } +static size_t -ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) _@(message.structure.namespaced_type.name)__get_serialized_key_size( const void * untyped_ros_message, size_t initial_alignment) { -@[ if message.structure.has_any_member_with_annotation('key') ]@ auto ros_message = static_cast( untyped_ros_message); @@ -777,30 +765,15 @@ _@(message.structure.namespaced_type.name)__get_serialized_key_size( @[end for]@ return current_alignment - initial_alignment; -@[ else]@ - static_cast(untyped_ros_message); - static_cast(initial_alignment); - return static_cast(0); -@[ end if]@ } -bool -ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) -get_key_type_support_@(message.structure.namespaced_type.name)( - message_type_support_key_callbacks_t * key_callbacks) -{ - std::cout << "Initializing CPP interface @(message.structure.namespaced_type.name)" << std::endl; -@[ if message.structure.has_any_member_with_annotation('key') ]@ - key_callbacks->max_serialized_key_size = &_@(message.structure.namespaced_type.name)__max_serialized_key_size; - key_callbacks->cdr_deserialize_key = &_@(message.structure.namespaced_type.name)__cdr_deserialize_key; - key_callbacks->cdr_serialize_key = &_@(message.structure.namespaced_type.name)__cdr_serialize_key; - key_callbacks->get_serialized_key_size = &_@(message.structure.namespaced_type.name)__get_serialized_key_size; - return true; -@[ else]@ - static_cast(key_callbacks); - return false; +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_key_size, + _@(message.structure.namespaced_type.name)__max_serialized_key_size, +}; @[ end if]@ -} static bool _@(message.structure.namespaced_type.name)__cdr_serialize( const void * untyped_ros_message, @@ -845,13 +818,6 @@ static size_t _@(message.structure.namespaced_type.name)__max_serialized_size(ch return ret_val; } -static bool _@(message.structure.namespaced_type.name)__get_key_type_support( message_type_support_key_callbacks_t * key_callbacks) -{ - bool ret_val; - ret_val = get_key_type_support_@(message.structure.namespaced_type.name)(key_callbacks); - return ret_val; -} - static message_type_support_callbacks_t _@(message.structure.namespaced_type.name)__callbacks = { "@('::'.join([package_name] + list(interface_path.parents[0].parts)))", "@(message.structure.namespaced_type.name)", @@ -859,11 +825,15 @@ static message_type_support_callbacks_t _@(message.structure.namespaced_type.nam _@(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)__get_key_type_support +@[ 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), From 4c441b81240f8ac8dbb016f61f7342ad2561aafb Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Thu, 15 Feb 2024 09:19:03 +0000 Subject: [PATCH 08/25] Refs #20151: Clean unsued old methods Signed-off-by: Mario Dominguez --- .../resource/msg__rosidl_typesupport_fastrtps_cpp.hpp.em | 6 ------ .../resource/msg__type_support.cpp.em | 3 --- 2 files changed, 9 deletions(-) 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 e96a57a..a8f68e3 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 @@ -84,12 +84,6 @@ max_serialized_size_@(message.structure.namespaced_type.name)( bool & full_bounded, bool & is_plain, size_t current_alignment); - -bool -ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) -get_key_type_support_@(message.structure.namespaced_type.name)( - message_type_support_key_callbacks_t * key_callbacks); - } // 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 28c739f..c815959 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -72,9 +72,6 @@ max_serialized_size_@(type_.name)( bool & full_bounded, bool & is_plain, size_t current_alignment); -bool -get_key_type_support_@(type_.name)( - message_type_support_key_callbacks_t * key_callbacks); } // namespace typesupport_fastrtps_cpp @[ for ns in reversed(type_.namespaces)]@ } // namespace @(ns) From 65eb3a4d442ffe0ab86bcf9a63617d44bc9836d6 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 09:32:33 +0000 Subject: [PATCH 09/25] Refs #20151: set _key as suffix in get_serialized and calculate_max_serialized Signed-off-by: Mario Dominguez --- .../rosidl_typesupport_fastrtps_cpp/message_type_support.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 9e81c40..c5b6e40 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 @@ -53,7 +53,7 @@ typedef struct message_type_support_key_callbacks_t /** * \return The size of the serialized message in bytes. */ - size_t (* get_serialized_key_size)( + size_t (* get_serialized_size_key)( const void * untyped_ros_message, size_t initial_alignment); @@ -64,7 +64,7 @@ typedef struct message_type_support_key_callbacks_t * \param [in,out] is_unbounded Whether the key has any unbounded member. * \return The maximum key serialized size, in bytes. */ - size_t (* max_serialized_key_size)( + size_t (* max_serialized_size_key)( size_t initial_aligment, bool & is_unbounded); From 76ce33575229586ff8946155b15c75b53f5488fb Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 09:41:04 +0000 Subject: [PATCH 10/25] Refs #20151: rosidl_typesupport_fastrtps_c: export key methods to be called from outside in headers Signed-off-by: Mario Dominguez --- .../resource/msg__rosidl_typesupport_fastrtps_c.h.em | 11 +++++++++++ 1 file changed, 11 insertions(+) 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..5ad62eb 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 @@ -37,6 +37,17 @@ 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) +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])))(); From ecd186e27f56d75a69897c7da6a0902da35b91f3 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 09:42:30 +0000 Subject: [PATCH 11/25] Refs #20151: rosidl_typesupport_fastrtps_c: source file forward declarations Signed-off-by: Mario Dominguez --- .../resource/msg__type_support_c.cpp.em | 15 +++++++++++++++ 1 file changed, 15 insertions(+) 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 5b3da8d..0900b38 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 @@ -151,6 +151,21 @@ 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]@ +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]@ From 57cb0234a60e78131bc45869855faa9bc4236a08 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 09:49:38 +0000 Subject: [PATCH 12/25] Refs #20151: rosidl_typesupport_fastrtps_c: generator refactor: generate_members_for_cdr_serialize Signed-off-by: Mario Dominguez --- .../resource/msg__type_support_c.cpp.em | 327 ++++++++---------- 1 file changed, 146 insertions(+), 181 deletions(-) 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 0900b38..aef277a 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 @@ -177,6 +177,134 @@ const rosidl_message_type_support_t * using _@(message.structure.namespaced_type.name)__ros_msg_type = @('__'.join(message.structure.namespaced_type.namespaced_name())); +@{ +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(type_, NamespacedType): + strlist.append(' const message_type_support_callbacks_t * callbacks =') + strlist.append(' static_cast(') + strlist.append(' ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(') + strlist.append(' rosidl_typesupport_fastrtps_c, %s' % (', '.join(type_.namespaced_name()))) + strlist.append(' )()->data);') + + 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) {') + if suffix == '': + strlist.append(' if (!callbacks->cdr_serialize(') + else: + strlist.append(' if (!callbacks->%s_callbacks->cdr_serialize%s(' % ((''.join(c for c in suffix if c not in '(){}<>_*')), suffix)) + strlist.append(' static_cast(&array_ptr[i]), cdr))') + strlist.append(' {') + strlist.append(' return false;') + strlist.append(' }') + 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) {') + if suffix == '': + strlist.append(' if (!callbacks->cdr_serialize(') + else: + strlist.append(' if (!callbacks->%s_callbacks->cdr_serialize%s(' % ((''.join(c for c in suffix if c not in '(){}<>_*')), suffix)) + strlist.append(' &array_ptr[i], cdr))') + strlist.append(' {') + strlist.append(' return false;') + strlist.append(' }') + 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: + if suffix == '': + strlist.append(' if (!callbacks->cdr_serialize(') + else: + strlist.append(' if (!callbacks->%s_callbacks->cdr_serialize%s(' % ((''.join(c for c in suffix if c not in '(){}<>_*')), suffix)) + strlist.append(' &ros_message->%s, cdr))' % (member.name)) + strlist.append(' {') + strlist.append(' return false;') + strlist.append(' }') + strlist.append('}') + + return strlist +}@ + static bool _@(message.structure.namespaced_type.name)__cdr_serialize( const void * untyped_ros_message, eprosima::fastcdr::Cdr & cdr) @@ -187,111 +315,9 @@ static bool _@(message.structure.namespaced_type.name)__cdr_serialize( } 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): - 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); - 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]@ - } +@[ for line in generate_member_for_cdr_serialize(member, '')]@ + @(line) +@[ end for]@ @[end for]@ return true; @@ -661,89 +687,28 @@ if isinstance(type_, AbstractNestedType): @[ if message.structure.has_any_member_with_annotation('key') ]@ static -size_t -_@(message.structure.namespaced_type.name)__max_serialized_key_size( - size_t initial_alignment, - bool & is_unbounded) +bool +_@(message.structure.namespaced_type.name)__cdr_serialize_key( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr) { - size_t current_alignment = initial_alignment; - is_unbounded = false; - - const size_t padding = 4; - const size_t wchar_size = 4; - (void)padding; - (void)wchar_size; + 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); + (void)ros_message; @[for member in message.structure.members]@ -@[ if not member.has_annotation('key')]@ +@[ if not member.has_annotation('key') and message.structure.has_any_member_with_annotation('key')]@ @[ continue]@ @[ end if]@ - // 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; -@[ end if]@ -@[ if isinstance(member.type, AbstractSequence)]@ - current_alignment += padding + - eprosima::fastcdr::Cdr::alignment(current_alignment, padding); -@[ end if]@ -@[ else]@ - size_t array_size = 1; -@[ end if]@ +@[ for line in generate_member_for_cdr_serialize(member, '_key')]@ + @(line) +@[ end for]@ -@{ -type_ = member.type -if isinstance(type_, AbstractNestedType): - type_ = type_.value_type -}@ -@[ if isinstance(type_, AbstractGenericString)]@ - is_unbounded = true; - 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')]@ - current_alignment += array_size * sizeof(uint8_t); -@[ elif type_.typename in ('wchar', 'int16', 'uint16')]@ - current_alignment += array_size * sizeof(uint16_t) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint16_t)); -@[ elif type_.typename in ('int32', 'uint32', 'float')]@ - current_alignment += array_size * sizeof(uint32_t) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t)); -@[ elif type_.typename in ('int64', 'uint64', 'double')]@ - current_alignment += array_size * sizeof(uint64_t) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t)); -@[ elif type_.typename == 'long double']@ - current_alignment += array_size * sizeof(long double) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(long double)); -@[ end if]@ -@[ else] - for (size_t index = 0; index < array_size; ++index) { - current_alignment += - _@(message.structure.namespaced_type.name)__max_serialized_key_size( - current_alignment, is_unbounded); - } -@[ end if]@ - } @[end for]@ - - return current_alignment - initial_alignment; + return true; } static From 0ad1b5c4e76c1f331898ddf5dee8fee00b5ac0ab Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 09:55:35 +0000 Subject: [PATCH 13/25] Refs #20151: rosidl_typesupport_fastrtps_c: generator refactor: generate_members_for_get_serialized_size Signed-off-by: Mario Dominguez --- .../resource/msg__type_support_c.cpp.em | 318 ++++++------------ 1 file changed, 102 insertions(+), 216 deletions(-) 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 aef277a..af09a81 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 @@ -482,6 +482,69 @@ else: return true; } // NOLINT(readability/fn_size) +@{ +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, @@ -497,60 +560,11 @@ 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; } @@ -720,129 +734,31 @@ _@(message.structure.namespaced_type.name)__cdr_deserialize_key( return false; } -static -bool -_@(message.structure.namespaced_type.name)__cdr_serialize_key( +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, - eprosima::fastcdr::Cdr & cdr) + size_t current_alignment) { -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); + (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')]@ +@[ if not member.has_annotation('key') and message.structure.has_any_member_with_annotation('key')]@ @[ continue]@ @[ end if]@ - // Field name: @(member.name) - { -@{ -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); - 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]@ - } +@[ for line in generate_member_for_get_serialized_size(member, '_key')]@ + @(line) +@[ end for]@ @[end for]@ - return true; + return current_alignment - initial_alignment; } static @@ -879,51 +795,14 @@ _@(message.structure.namespaced_type.name)__get_serialized_key_size( } @[ end if]@ - 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 += - _@(message.structure.namespaced_type.name)__get_serialized_key_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 += - _@(message.structure.namespaced_type.name)__get_serialized_key_size( - ros_message->@(member.name), current_alignment); -@[ end if]@ -@[ end if]@ -@[end for]@ +@[ if message.structure.has_any_member_with_annotation('key') ]@ +static size_t _@(message.structure.namespaced_type.name)__get_serialized_size_key( + const void * untyped_ros_message, + size_t initial_aligment) +{ + return get_serialized_size_key_@('__'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name]))( + untyped_ros_message, initial_aligment); +} return current_alignment - initial_alignment; } @@ -936,6 +815,16 @@ static message_type_support_key_callbacks_t __key_callbacks_@(message.structure. }; @[ end if]@ +@ +@# // Collect the callback functions and provide a function to get the type support struct. + +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; @@ -951,9 +840,6 @@ 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)", From 7c87bf1489bbad24baf91b9f41863bed3670c340 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 09:57:00 +0000 Subject: [PATCH 14/25] Refs #20151: rosidl_typesupport_fastrtps_c: generator refactor: generate_members_for_max_serialized_size Signed-off-by: Mario Dominguez --- .../resource/msg__type_support_c.cpp.em | 253 ++++++++++-------- 1 file changed, 144 insertions(+), 109 deletions(-) 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 af09a81..08fa090 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 @@ -568,12 +568,93 @@ size_t get_serialized_size_@('__'.join([package_name] + list(interface_path.pare 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)); -} +@{ +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]))( @@ -600,85 +681,10 @@ 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; -@[ end if]@ +@[ for line in generate_member_for_max_serialized_size(member, '')]@ + @(line) +@[ end for]@ -@{ -type_ = member.type -if isinstance(type_, AbstractNestedType): - type_ = type_.value_type -}@ -@[ if isinstance(type_, AbstractGenericString)]@ - full_bounded = false; - is_plain = false; - for (size_t index = 0; index < array_size; ++index) { - current_alignment += padding + - eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + -@[ if type_.has_maximum_size()]@ -@[ if isinstance(type_, AbstractWString)]@ - wchar_size * -@[ end if]@ - @(type_.maximum_size) + -@[ end if]@ -@[ if isinstance(type_, AbstractWString)]@ - wchar_size * -@[ end if]@ - 1; - } -@[ 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; - } -@[ end if]@ - } @[end for]@ size_t ret_val = current_alignment - initial_alignment; @@ -694,7 +700,6 @@ if isinstance(type_, AbstractNestedType): last_member_size ) == ret_val; } - @[end if]@ return ret_val; } @@ -761,39 +766,55 @@ size_t get_serialized_size_key_@('__'.join([package_name] + list(interface_path. return current_alignment - initial_alignment; } -static -size_t -_@(message.structure.namespaced_type.name)__get_serialized_key_size( - const void * untyped_ros_message, - size_t 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) { - auto ros_message = - static_cast( - untyped_ros_message); - - size_t current_alignment = initial_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; + +@{ +last_member_name_ = None +}@ @[for member in message.structure.members]@ -@[ if not member.has_annotation('key')]@ +@{ +last_member_name_ = member.name +}@ +@[ if not member.has_annotation('key') and message.structure.has_any_member_with_annotation('key')]@ @[ continue]@ @[ end if]@ - // 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_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) { + // 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; +} @[ if message.structure.has_any_member_with_annotation('key') ]@ static size_t _@(message.structure.namespaced_type.name)__get_serialized_size_key( @@ -804,7 +825,21 @@ static size_t _@(message.structure.namespaced_type.name)__get_serialized_size_ke untyped_ros_message, initial_aligment); } - return current_alignment - initial_alignment; +static +size_t +_@(message.structure.namespaced_type.name)__max_serialized_size_key( + size_t initial_alignment, + 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, initial_alignment); + + is_unbounded = !full_bounded; + return ret_val; } static message_type_support_key_callbacks_t __key_callbacks_@(message.structure.namespaced_type.name) = { From bb6133992409770710e5d1ae9a9b31d66000799e Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 09:58:18 +0000 Subject: [PATCH 15/25] Refs #20151: rosidl_typesupport_fastrtps_c: update suffix in __key_callback structure Signed-off-by: Mario Dominguez --- .../resource/msg__type_support_c.cpp.em | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 08fa090..9a0b5a8 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 @@ -845,8 +845,8 @@ _@(message.structure.namespaced_type.name)__max_serialized_size_key( 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_key_size, - _@(message.structure.namespaced_type.name)__max_serialized_key_size, + _@(message.structure.namespaced_type.name)__get_serialized_size_key, + _@(message.structure.namespaced_type.name)__max_serialized_size_key, }; @[ end if]@ From 155e31f0a399844c989ba427ea3589834e95627d Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 10:07:38 +0000 Subject: [PATCH 16/25] Refs #20151: rosidl_typesupport_fastrtps_c: avoid generating cdr_serialize_key and cdr_deserialize_key methods that are not going to be used nor exported if the type do not have keys Signed-off-by: Mario Dominguez --- .../resource/msg__type_support_c.cpp.em | 1 + 1 file changed, 1 insertion(+) 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 9a0b5a8..34c92c4 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 @@ -738,6 +738,7 @@ _@(message.structure.namespaced_type.name)__cdr_deserialize_key( { return false; } +@[ end if]@ 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]))( From da232fedacebb1caefef5b6ba483b4844d4c3185 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 10:20:00 +0000 Subject: [PATCH 17/25] Refs #20151: rosidl_typesupport_fastrtps_cpp: export key methods to be called from outside in headers Signed-off-by: Mario Dominguez --- ...sg__rosidl_typesupport_fastrtps_cpp.hpp.em | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) 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 a8f68e3..34c563d 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 @@ -84,6 +84,32 @@ max_serialized_size_@(message.structure.namespaced_type.name)( bool & full_bounded, 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)]@ From bdeaa95693f78c5579578b32a3d3e5273c91fb38 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 10:20:32 +0000 Subject: [PATCH 18/25] Refs #20151: rosidl_typesupport_fastrtps_cpp: source file forward declarations Signed-off-by: Mario Dominguez --- .../resource/msg__type_support.cpp.em | 14 ++++++++++++++ 1 file changed, 14 insertions(+) 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 c815959..d4d9616 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) From 01b1ec16c6bc462684649f6534db2c9f87960bf5 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 10:33:47 +0000 Subject: [PATCH 19/25] Refs #20151: rosidl_typesupport_fastrtps_cpp: generator refactor: generate_members_for_cdr_serialize. Also, generate proxy methods only when the type has a key Signed-off-by: Mario Dominguez --- .../resource/msg__type_support.cpp.em | 280 ++++++++---------- 1 file changed, 117 insertions(+), 163 deletions(-) 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 d4d9616..6bc7f7d 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -104,6 +104,89 @@ namespace @(ns) @{forward_declared_types.add(message.structure.namespaced_type.namespaced_name())}@ namespace typesupport_fastrtps_cpp { +@{ +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) @@ -112,85 +195,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; } @@ -405,85 +413,10 @@ 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 if]@ - -@{ -type_ = member.type -if isinstance(type_, AbstractNestedType): - type_ = type_.value_type -}@ -@[ if isinstance(type_, AbstractGenericString)]@ - full_bounded = false; - is_plain = false; - for (size_t index = 0; index < array_size; ++index) { - current_alignment += padding + - eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + -@[ if type_.has_maximum_size()]@ -@[ if isinstance(type_, AbstractWString)]@ - wchar_size * -@[ end if]@ - @(type_.maximum_size) + -@[ end if]@ -@[ if isinstance(type_, AbstractWString)]@ - wchar_size * -@[ end if]@ - 1; - } -@[ 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; - } -@[ end if]@ - } @[end for]@ size_t ret_val = current_alignment - initial_alignment; @@ -504,6 +437,24 @@ if isinstance(type_, AbstractNestedType): 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; +} + @[ if message.structure.has_any_member_with_annotation('key') ]@ static size_t @@ -589,13 +540,16 @@ if isinstance(type_, AbstractNestedType): return current_alignment - initial_alignment; } -static -bool -_@(message.structure.namespaced_type.name)__cdr_deserialize_key( - eprosima::fastcdr::Cdr &, - void *) +@[ 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) { - return false; + auto typed_message = + static_cast( + untyped_ros_message); + + return cdr_serialize_key(*typed_message, cdr); } static From b769e7b316ff1aff933b071ea58fd0ae5aa8c763 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 10:36:05 +0000 Subject: [PATCH 20/25] Refs #20151: rosidl_typesupport_fastrtps_cpp: generator refactor: generate_members_for_get_serialized_size adn proxy Signed-off-by: Mario Dominguez --- .../resource/msg__type_support.cpp.em | 199 ++++++++---------- 1 file changed, 93 insertions(+), 106 deletions(-) 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 6bc7f7d..66982d2 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -311,6 +311,73 @@ cdr_deserialize( return true; } +@{ +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( @@ -325,65 +392,11 @@ 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; } @@ -458,34 +471,29 @@ cdr_serialize_key( @[ if message.structure.has_any_member_with_annotation('key') ]@ static size_t -_@(message.structure.namespaced_type.name)__max_serialized_key_size( - size_t initial_alignment, - bool & is_unbounded) +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 current_alignment = initial_alignment; - is_unbounded = false; + 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')]@ +@[ if not member.has_annotation('key') and message.structure.has_any_member_with_annotation('key')]@ @[ continue]@ @[ end if]@ - // 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; -@[ end if]@ -@[ if isinstance(member.type, AbstractSequence)]@ - const size_t padding = 4; - current_alignment += padding + - eprosima::fastcdr::Cdr::alignment(current_alignment, padding); -@[ end if]@ -@[ else]@ - size_t array_size = 1; -@[ end if]@ +@[ for line in generate_member_for_get_serialized_size(member, '_key')]@ + @(line) +@[ end for]@ + +@[end for]@ + return current_alignment - initial_alignment; +} @{ type_ = member.type @@ -651,37 +659,16 @@ _@(message.structure.namespaced_type.name)__cdr_serialize_key( static size_t -_@(message.structure.namespaced_type.name)__get_serialized_key_size( +_@(message.structure.namespaced_type.name)__get_serialized_size_key( const void * untyped_ros_message, size_t initial_alignment) { - auto ros_message = + auto typed_message = static_cast( untyped_ros_message); - size_t current_alignment = initial_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')]@ -@[ continue]@ -@[ end if]@ - // 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]@ + return static_cast(get_serialized_size_key(*typed_message, initial_alignment)); +} current_alignment += padding + eprosima::fastcdr::Cdr::alignment(current_alignment, padding); From 46208a3c19cd6ae3b2bc050c2303e438a284799b Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 10:37:28 +0000 Subject: [PATCH 21/25] Refs #20151: rosidl_typesupport_fastrtps_cpp: generator refactor: generate_members_for_max_serialized_size and proxy Signed-off-by: Mario Dominguez --- .../resource/msg__type_support.cpp.em | 238 +++++++++++------- 1 file changed, 144 insertions(+), 94 deletions(-) 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 66982d2..2173e14 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -400,6 +400,92 @@ get_serialized_size( return current_alignment - initial_alignment; } +@{ +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)( @@ -445,7 +531,6 @@ last_member_name_ = member.name last_member_size ) == ret_val; } - @[end if]@ return ret_val; } @@ -495,57 +580,55 @@ get_serialized_size_key( 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)]@ - is_unbounded = true; - const size_t padding = 4; - const size_t wchar_size = 4; - - 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')]@ - current_alignment += array_size * sizeof(uint8_t); -@[ elif type_.typename in ('wchar', 'int16', 'uint16')]@ - current_alignment += array_size * sizeof(uint16_t) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint16_t)); -@[ elif type_.typename in ('int32', 'uint32', 'float')]@ - current_alignment += array_size * sizeof(uint32_t) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t)); -@[ elif type_.typename in ('int64', 'uint64', 'double')]@ - current_alignment += array_size * sizeof(uint64_t) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t)); -@[ elif type_.typename == 'long double']@ - current_alignment += array_size * sizeof(long double) + - eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(long double)); -@[ end if]@ -@[ else] - for (size_t index = 0; index < array_size; ++index) { - current_alignment += - @('::'.join(type_.namespaces))::typesupport_fastrtps_cpp::_@(message.structure.namespaced_type.name)__max_serialized_key_size( - current_alignment, is_unbounded); - } +@[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]@ - return current_alignment - initial_alignment; +@[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; } @[ if message.structure.has_any_member_with_annotation('key') ]@ @@ -670,53 +753,20 @@ _@(message.structure.namespaced_type.name)__get_serialized_size_key( return static_cast(get_serialized_size_key(*typed_message, initial_alignment)); } - 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::_@(message.structure.namespaced_type.name)__get_serialized_key_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::_@(message.structure.namespaced_type.name)__get_serialized_key_size( - ros_message->@(member.name), current_alignment); -@[ end if]@ -@[ end if]@ -@[end for]@ +static size_t _@(message.structure.namespaced_type.name)__max_serialized_size_key( + size_t current_alignment, + bool & is_unbounded) +{ + bool full_bounded = true; + bool is_plain = true; - return current_alignment - initial_alignment; + size_t ret_val = max_serialized_size_key_@(message.structure.namespaced_type.name)( + full_bounded, + is_plain, + current_alignment); + + is_unbounded = !full_bounded; + return ret_val; } static message_type_support_key_callbacks_t _@(message.structure.namespaced_type.name)__key_callbacks = { From 09504e81c55b85587c6a007372030b3a6ceb92c1 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 10:38:47 +0000 Subject: [PATCH 22/25] Refs #20151: rosidl_typesupport_fastrtps_cpp: dummy cdr_deserialize_key method and proxy Signed-off-by: Mario Dominguez --- .../resource/msg__type_support.cpp.em | 110 +++--------------- 1 file changed, 18 insertions(+), 92 deletions(-) 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 2173e14..db4d653 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -553,8 +553,18 @@ cdr_serialize_key( return true; } -@[ if message.structure.has_any_member_with_annotation('key') ]@ -static +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; + + return false; +} + size_t ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(package_name) get_serialized_size_key( @@ -645,99 +655,15 @@ static bool _@(message.structure.namespaced_type.name)__cdr_serialize_key( static bool -_@(message.structure.namespaced_type.name)__cdr_serialize_key( - const void * untyped_ros_message, - eprosima::fastcdr::Cdr & cdr) +_@(message.structure.namespaced_type.name)__cdr_deserialize_key( + eprosima::fastcdr::Cdr & cdr, + void * untyped_ros_message) { - auto ros_message = - static_cast( + auto typed_message = + static_cast<@('::'.join([package_name] + list(interface_path.parents[0].parts) + [message.structure.namespaced_type.name])) *>( untyped_ros_message); -@[for member in message.structure.members]@ -@[ if not member.has_annotation('key')]@ -@[ continue]@ -@[ end if]@ - // 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("key 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]@ -@[end for]@ - return true; + return cdr_deserialize_key(cdr, *typed_message); } static From af48357cdcf8a0209583ab59dad837089558f33a Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Fri, 23 Feb 2024 10:39:07 +0000 Subject: [PATCH 23/25] Refs #20151: rosidl_typesupport_fastrtps_cpp: update suffix in __key_callback structure Signed-off-by: Mario Dominguez --- .../resource/msg__type_support.cpp.em | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 db4d653..d793c36 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -698,8 +698,8 @@ static size_t _@(message.structure.namespaced_type.name)__max_serialized_size_ke 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_key_size, - _@(message.structure.namespaced_type.name)__max_serialized_key_size, + _@(message.structure.namespaced_type.name)__get_serialized_size_key, + _@(message.structure.namespaced_type.name)__max_serialized_size_key, }; @[ end if]@ From 0400104324d1347b6a32c6d2883404354c3f4f25 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Wed, 28 Feb 2024 15:13:16 +0000 Subject: [PATCH 24/25] Refs #20310: Review 2 suggestions on rosidl_typesupport_fastrtps_cpp Signed-off-by: Mario Dominguez --- .../message_type_support.h | 14 +++++------ ...sg__rosidl_typesupport_fastrtps_cpp.hpp.em | 1 - .../resource/msg__type_support.cpp.em | 25 ++++++++++++++----- 3 files changed, 25 insertions(+), 15 deletions(-) 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 c5b6e40..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 @@ -29,7 +29,7 @@ // Holds generated methods related with keys typedef struct message_type_support_key_callbacks_t { - /// Callback function for message serialization + /// Callback function for key serialization /** * \param[in] untyped_ros_message Type erased pointer to message instance. * \param [in,out] Serialized FastCDR data object. @@ -39,10 +39,10 @@ typedef struct message_type_support_key_callbacks_t const void * untyped_ros_message, eprosima::fastcdr::Cdr & cdr); - /// Callback function for message deserialization + /// Callback function for key deserialization /** * \param [in] Serialized FastCDR data object. - * \param[out] untyped_ros_message Type erased pointer to message instance. + * \param [out] untyped_ros_message Type erased pointer to message instance. * \return true if deserialization succeeded, false otherwise. */ bool (* cdr_deserialize_key)( @@ -51,21 +51,19 @@ typedef struct message_type_support_key_callbacks_t /// Callback function to get size of the key data /** - * \return The size of the serialized message in bytes. + * \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, - size_t initial_alignment); + 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] initial_alignment Initial alignment to be incrementally calculated. * \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)( - size_t initial_aligment, bool & is_unbounded); } message_type_support_key_callbacks_t; 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 34c563d..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 @@ -50,7 +50,6 @@ header_files = [ #include "@(header_file)" @[end for]@ -struct message_type_support_key_callbacks_t; @[for ns in message.structure.namespaced_type.namespaces]@ namespace @(ns) 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 d793c36..ec4208a 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -105,6 +105,11 @@ namespace @(ns) 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 @@ -312,6 +317,11 @@ cdr_deserialize( } @{ + +# 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 @@ -401,6 +411,11 @@ get_serialized_size( } @{ + +# 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 @@ -561,7 +576,7 @@ cdr_deserialize_key( { (void)ros_message; (void)cdr; - + // TODO return false; } @@ -669,18 +684,16 @@ _@(message.structure.namespaced_type.name)__cdr_deserialize_key( static size_t _@(message.structure.namespaced_type.name)__get_serialized_size_key( - const void * untyped_ros_message, - size_t initial_alignment) + const void * untyped_ros_message) { auto typed_message = static_cast( untyped_ros_message); - return static_cast(get_serialized_size_key(*typed_message, initial_alignment)); + return get_serialized_size_key(*typed_message, 0); } static size_t _@(message.structure.namespaced_type.name)__max_serialized_size_key( - size_t current_alignment, bool & is_unbounded) { bool full_bounded = true; @@ -689,7 +702,7 @@ static size_t _@(message.structure.namespaced_type.name)__max_serialized_size_ke size_t ret_val = max_serialized_size_key_@(message.structure.namespaced_type.name)( full_bounded, is_plain, - current_alignment); + 0); is_unbounded = !full_bounded; return ret_val; From 491fea297a2ce36f8b3bffdf98510f388a1bd2b3 Mon Sep 17 00:00:00 2001 From: Mario Dominguez Date: Wed, 28 Feb 2024 15:13:48 +0000 Subject: [PATCH 25/25] Refs #20310: Review 2 suggestions on rosidl_typesupport_fastrtps_c Signed-off-by: Mario Dominguez --- .../msg__rosidl_typesupport_fastrtps_c.h.em | 29 +++ .../resource/msg__type_support_c.cpp.em | 208 +++++++++++------- 2 files changed, 152 insertions(+), 85 deletions(-) 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 5ad62eb..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,16 @@ 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, 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 34c92c4..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,20 @@ 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]@ @@ -178,6 +207,11 @@ const rosidl_message_type_support_t * using _@(message.structure.namespaced_type.name)__ros_msg_type = @('__'.join(message.structure.namespaced_type.namespaced_name())); @{ + +# 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 @@ -198,13 +232,6 @@ def generate_member_for_cdr_serialize(member, suffix): if isinstance(type_, AbstractNestedType): type_ = type_.value_type - if isinstance(type_, NamespacedType): - strlist.append(' const message_type_support_callbacks_t * callbacks =') - strlist.append(' static_cast(') - strlist.append(' ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(') - strlist.append(' rosidl_typesupport_fastrtps_c, %s' % (', '.join(type_.namespaced_name()))) - strlist.append(' )()->data);') - if isinstance(member.type, AbstractNestedType): if isinstance(member.type, Array): strlist.append(' size_t size = %d;' % (member.type.size)) @@ -248,27 +275,15 @@ def generate_member_for_cdr_serialize(member, suffix): 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) {') - if suffix == '': - strlist.append(' if (!callbacks->cdr_serialize(') - else: - strlist.append(' if (!callbacks->%s_callbacks->cdr_serialize%s(' % ((''.join(c for c in suffix if c not in '(){}<>_*')), suffix)) - strlist.append(' static_cast(&array_ptr[i]), cdr))') - strlist.append(' {') - strlist.append(' return false;') - strlist.append(' }') + 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) {') - if suffix == '': - strlist.append(' if (!callbacks->cdr_serialize(') - else: - strlist.append(' if (!callbacks->%s_callbacks->cdr_serialize%s(' % ((''.join(c for c in suffix if c not in '(){}<>_*')), suffix)) - strlist.append(' &array_ptr[i], cdr))') - strlist.append(' {') - strlist.append(' return false;') - strlist.append(' }') + 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)) @@ -292,28 +307,18 @@ def generate_member_for_cdr_serialize(member, suffix): elif isinstance(member.type, BasicType): strlist.append(' cdr << ros_message->%s;' % (member.name)) else: - if suffix == '': - strlist.append(' if (!callbacks->cdr_serialize(') - else: - strlist.append(' if (!callbacks->%s_callbacks->cdr_serialize%s(' % ((''.join(c for c in suffix if c not in '(){}<>_*')), suffix)) - strlist.append(' &ros_message->%s, cdr))' % (member.name)) - strlist.append(' {') - strlist.append(' return false;') - strlist.append(' }') + strlist.append(' cdr_serialize%s_%s(' % (suffix, ('__'.join(member.type.namespaced_name())))) + strlist.append(' &ros_message->%s, cdr);' % (member.name)) strlist.append('}') return strlist }@ -static bool _@(message.structure.namespaced_type.name)__cdr_serialize( - const void * untyped_ros_message, +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) { - 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]@ @[ for line in generate_member_for_cdr_serialize(member, '')]@ @(line) @@ -323,15 +328,11 @@ static bool _@(message.structure.namespaced_type.name)__cdr_serialize( 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) { @@ -340,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); @@ -427,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)]@ @@ -470,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]@ } @@ -483,6 +469,11 @@ else: } // 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 @@ -569,6 +560,11 @@ size_t get_serialized_size_@('__'.join([package_name] + list(interface_path.pare } @{ + +# 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 @@ -704,20 +700,11 @@ last_member_name_ = member.name 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, +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) { - 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); - (void)ros_message; - @[for member in message.structure.members]@ @[ if not member.has_annotation('key') and message.structure.has_any_member_with_annotation('key')]@ @[ continue]@ @@ -730,15 +717,16 @@ _@(message.structure.namespaced_type.name)__cdr_serialize_key( return true; } -static -bool -_@(message.structure.namespaced_type.name)__cdr_deserialize_key( - eprosima::fastcdr::Cdr &, - void *) +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; } -@[ end if]@ 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]))( @@ -818,18 +806,42 @@ last_member_name_ = member.name } @[ if message.structure.has_any_member_with_annotation('key') ]@ -static size_t _@(message.structure.namespaced_type.name)__get_serialized_size_key( +static bool _@(message.structure.namespaced_type.name)__cdr_serialize_key( const void * untyped_ros_message, - size_t initial_aligment) + 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, initial_aligment); + untyped_ros_message, 0); } static size_t _@(message.structure.namespaced_type.name)__max_serialized_size_key( - size_t initial_alignment, bool & is_unbounded) { bool full_bounded; @@ -837,7 +849,7 @@ _@(message.structure.namespaced_type.name)__max_serialized_size_key( 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, initial_alignment); + full_bounded, is_plain, 0); is_unbounded = !full_bounded; return ret_val; @@ -854,6 +866,32 @@ static message_type_support_key_callbacks_t __key_callbacks_@(message.structure. @ @# // 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(