From f629b2f96ba56c7232a8f74ab81d44bf4cc777f2 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 15 May 2019 16:14:44 -0700 Subject: [PATCH] add support for WString in rmw_fastrtps_dynamic_cpp (#278) * add support for WString in rmw_fastrtps_dynamic_cpp Signed-off-by: Dirk Thomas * fix compiler warning on Windows Signed-off-by: Dirk Thomas --- rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 4 + .../TypeSupport_impl.hpp | 210 +++++++++++++++++- rmw_fastrtps_dynamic_cpp/package.xml | 4 + 3 files changed, 206 insertions(+), 12 deletions(-) diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index 51f335679..0225cc8d8 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -41,6 +41,8 @@ find_package(FastRTPS REQUIRED MODULE) find_package(rmw REQUIRED) find_package(rosidl_generator_c REQUIRED) +find_package(rosidl_typesupport_fastrtps_c REQUIRED) +find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) find_package(rosidl_typesupport_introspection_c REQUIRED) find_package(rosidl_typesupport_introspection_cpp REQUIRED) @@ -89,6 +91,8 @@ target_link_libraries(rmw_fastrtps_dynamic_cpp # specific order: dependents before dependencies ament_target_dependencies(rmw_fastrtps_dynamic_cpp "rcutils" + "rosidl_typesupport_fastrtps_c" + "rosidl_typesupport_fastrtps_cpp" "rosidl_typesupport_introspection_c" "rosidl_typesupport_introspection_cpp" "rmw_fastrtps_shared_cpp" diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp index e27f1ae92..93a5f53c0 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp @@ -23,6 +23,8 @@ #include "rmw_fastrtps_dynamic_cpp/TypeSupport.hpp" #include "rmw_fastrtps_dynamic_cpp/macros.hpp" +#include "rosidl_typesupport_fastrtps_c/wstring_conversion.hpp" +#include "rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp" #include "rosidl_typesupport_introspection_cpp/field_types.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" #include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" @@ -31,6 +33,7 @@ #include "rosidl_typesupport_introspection_c/service_introspection.h" #include "rosidl_generator_c/primitives_sequence_functions.h" +#include "rosidl_generator_c/u16string_functions.h" namespace rmw_fastrtps_dynamic_cpp { @@ -178,6 +181,15 @@ static size_t calculateMaxAlign(const MembersType * members) alignment = alignof(std::string); } break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + if (std::is_same::value) + { + alignment = alignof(rosidl_generator_c__U16String); + } else { + alignment = alignof(std::u16string); + } + break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: { auto sub_members = (const MembersType *)member.members_->data; @@ -212,6 +224,35 @@ void serialize_field( } } +template<> +inline +void serialize_field( + const rosidl_typesupport_introspection_cpp::MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & ser) +{ + std::wstring wstr; + if (!member->is_array_) { + auto u16str = static_cast(field); + rosidl_typesupport_fastrtps_cpp::u16string_to_wstring(*u16str, wstr); + ser << wstr; + } else { + size_t size; + if (member->array_size_ && !member->is_upper_bound_) { + size = member->array_size_; + } else { + size = member->size_function(field); + ser << static_cast(size); + } + for (size_t i = 0; i < size; ++i) { + const void * element = member->get_const_function(field, i); + auto u16str = static_cast(element); + rosidl_typesupport_fastrtps_cpp::u16string_to_wstring(*u16str, wstr); + ser << wstr; + } + } +} + // C specialization template void serialize_field( @@ -269,6 +310,33 @@ void serialize_field( } } +template<> +inline +void serialize_field( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & ser) +{ + std::wstring wstr; + if (!member->is_array_) { + auto u16str = static_cast(field); + rosidl_typesupport_fastrtps_c::u16string_to_wstring(*u16str, wstr); + ser << wstr; + } else if (member->array_size_ && !member->is_upper_bound_) { + auto array = static_cast(field); + for (size_t i = 0; i < member->array_size_; ++i) { + rosidl_typesupport_fastrtps_c::u16string_to_wstring(array[i], wstr); + ser << wstr; + } + } else { + auto sequence = static_cast(field); + ser << static_cast(sequence->size); + for (size_t i = 0; i < sequence->size; ++i) { + rosidl_typesupport_fastrtps_c::u16string_to_wstring(sequence->data[i], wstr); + ser << wstr; + } + } +} inline size_t get_array_size_and_assign_field( const rosidl_typesupport_introspection_cpp::MessageMember * member, @@ -357,6 +425,9 @@ bool TypeSupport::serializeROSmessage( case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: serialize_field(member, field, ser); break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + serialize_field(member, field, ser); + break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: { auto sub_members = static_cast(member->members_->data); @@ -422,34 +493,35 @@ size_t next_field_align( return current_alignment; } -template<> -inline -size_t next_field_align( +template +size_t next_field_align_string( const rosidl_typesupport_introspection_cpp::MessageMember * member, void * field, size_t current_alignment) { const size_t padding = 4; + size_t character_size = + (member->type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING) ? 4 : 1; if (!member->is_array_) { current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); current_alignment += padding; - auto & str = *static_cast(field); - current_alignment += str.size() + 1; + auto & str = *static_cast(field); + current_alignment += character_size * (str.size() + 1); } else if (member->array_size_ && !member->is_upper_bound_) { - auto str_arr = static_cast(field); + auto str_arr = static_cast(field); for (size_t index = 0; index < member->array_size_; ++index) { current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); current_alignment += padding; - current_alignment += str_arr[index].size() + 1; + current_alignment += character_size * (str_arr[index].size() + 1); } } else { - auto & data = *reinterpret_cast *>(field); + auto & data = *reinterpret_cast *>(field); current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); current_alignment += padding; for (auto & it : data) { current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); current_alignment += padding; - current_alignment += it.size() + 1; + current_alignment += character_size * (it.size() + 1); } } return current_alignment; @@ -481,9 +553,15 @@ size_t next_field_align( return current_alignment; } +template +size_t next_field_align_string( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + size_t current_alignment); + template<> inline -size_t next_field_align( +size_t next_field_align_string( const rosidl_typesupport_introspection_c__MessageMember * member, void * field, size_t current_alignment) @@ -514,6 +592,42 @@ size_t next_field_align( return current_alignment; } +template<> +inline +size_t next_field_align_string( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + size_t current_alignment) +{ + const size_t padding = 4; + if (!member->is_array_) { + auto u16str = static_cast(field); + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + current_alignment += 4 * (u16str->size + 1); + } else { + if (member->array_size_ && !member->is_upper_bound_) { + auto string_field = static_cast(field); + for (size_t i = 0; i < member->array_size_; ++i) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + current_alignment += 4 * (string_field[i].size + 1); + } + } else { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + auto & string_sequence_field = + *reinterpret_cast(field); + for (size_t i = 0; i < string_sequence_field.size; ++i) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + current_alignment += 4 * (string_sequence_field.data[i].size + 1); + } + } + } + return current_alignment; +} + template size_t TypeSupport::getEstimatedSerializedSize( const MembersType * members, const void * ros_message, size_t current_alignment) @@ -563,7 +677,10 @@ size_t TypeSupport::getEstimatedSerializedSize( current_alignment = next_field_align(member, field, current_alignment); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: - current_alignment = next_field_align(member, field, current_alignment); + current_alignment = next_field_align_string(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + current_alignment = next_field_align_string(member, field, current_alignment); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: { @@ -656,6 +773,36 @@ inline void deserialize_field( } } +template<> +inline void deserialize_field( + const rosidl_typesupport_introspection_cpp::MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & deser, + bool call_new) +{ + (void)call_new; + std::wstring wstr; + if (!member->is_array_) { + deser >> wstr; + rosidl_typesupport_fastrtps_cpp::wstring_to_u16string( + wstr, *static_cast(field)); + } else { + uint32_t size; + if (member->array_size_ && !member->is_upper_bound_) { + size = static_cast(member->array_size_); + } else { + deser >> size; + member->resize_function(field, size); + } + for (size_t i = 0; i < size; ++i) { + void * element = member->get_function(field, i); + auto u16str = static_cast(element); + deser >> wstr; + rosidl_typesupport_fastrtps_cpp::wstring_to_u16string(wstr, *u16str); + } + } +} + template void deserialize_field( const rosidl_typesupport_introspection_c__MessageMember * member, @@ -724,6 +871,39 @@ inline void deserialize_field( } } +template<> +inline void deserialize_field( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & deser, + bool call_new) +{ + (void)call_new; + std::wstring wstr; + if (!member->is_array_) { + deser >> wstr; + rosidl_typesupport_fastrtps_c::wstring_to_u16string( + wstr, *static_cast(field)); + } else if (member->array_size_ && !member->is_upper_bound_) { + auto array = static_cast(field); + for (size_t i = 0; i < member->array_size_; ++i) { + deser >> wstr; + rosidl_typesupport_fastrtps_c::wstring_to_u16string(wstr, array[i]); + } + } else { + uint32_t size; + deser >> size; + auto sequence = static_cast(field); + if (!rosidl_generator_c__U16String__Sequence__init(sequence, size)) { + throw std::runtime_error("unable to initialize rosidl_generator_c__U16String sequence"); + } + for (size_t i = 0; i < sequence->size; ++i) { + deser >> wstr; + rosidl_typesupport_fastrtps_c::wstring_to_u16string(wstr, sequence->data[i]); + } + } +} + inline size_t get_submessage_array_deserialize( const rosidl_typesupport_introspection_cpp::MessageMember * member, eprosima::fastcdr::Cdr & deser, @@ -815,6 +995,9 @@ bool TypeSupport::deserializeROSmessage( case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: deserialize_field(member, field, deser, call_new); break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + deserialize_field(member, field, deser, call_new); + break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: { auto sub_members = (const MembersType *)member->members_->data; @@ -903,12 +1086,15 @@ size_t TypeSupport::calculateMaxSerializedSize( eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t)); break; case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: { this->max_size_bound_ = false; + size_t character_size = + (member->type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING) ? 4 : 1; for (size_t index = 0; index < array_size; ++index) { current_alignment += padding + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + - member->string_upper_bound_ + 1; + character_size * (member->string_upper_bound_ + 1); } } break; diff --git a/rmw_fastrtps_dynamic_cpp/package.xml b/rmw_fastrtps_dynamic_cpp/package.xml index 51e651242..ec8e11c5a 100644 --- a/rmw_fastrtps_dynamic_cpp/package.xml +++ b/rmw_fastrtps_dynamic_cpp/package.xml @@ -20,6 +20,8 @@ rmw rmw_fastrtps_shared_cpp rosidl_generator_c + rosidl_typesupport_fastrtps_c + rosidl_typesupport_fastrtps_cpp rosidl_typesupport_introspection_c rosidl_typesupport_introspection_cpp @@ -30,6 +32,8 @@ rmw rmw_fastrtps_shared_cpp rosidl_generator_c + rosidl_typesupport_fastrtps_c + rosidl_typesupport_fastrtps_cpp rosidl_typesupport_introspection_c rosidl_typesupport_introspection_cpp