From b4f63c02453b8a8afb5e996229d309cb6d1c41f5 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 20 Mar 2024 19:30:17 +0100 Subject: [PATCH] Improve wide string (de)serialization (#113) * Added serialization helpers for char16_t strings in rosidl_typesupport_fastrtps_c. * Change generated code in rosidl_typesupport_fastrtps_c * Added serialization helpers for char16_t strings in rosidl_typesupport_fastrtps_cpp. * Change generated code in rosidl_typesupport_fastrtps_cpp * Throw overflow_error when size does not fit. * Throw exception when deserialized char is too big. * Deprecate unused methods * Fix dependency on fastcdr Signed-off-by: Miguel Company --- rosidl_typesupport_fastrtps_c/CMakeLists.txt | 3 + .../serialization_helpers.hpp | 81 +++++++++++++++++++ .../wstring_conversion.hpp | 3 + .../resource/msg__type_support_c.cpp.em | 16 ++-- .../benchmark_string_conversions.cpp | 25 ++++-- .../test/test_wstring_conversion.cpp | 53 ++++++------ .../test/test_wstring_conversion_mem.cpp | 18 ++++- .../serialization_helpers.hpp | 78 ++++++++++++++++++ .../wstring_conversion.hpp | 4 + .../resource/msg__type_support.cpp.em | 39 +++------ .../benchmark_string_conversions.cpp | 21 ++++- .../test/test_wstring_conversion.cpp | 45 +++++------ .../test/test_wstring_conversion_mem.cpp | 18 ++++- 13 files changed, 301 insertions(+), 103 deletions(-) create mode 100644 rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/serialization_helpers.hpp create mode 100644 rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp diff --git a/rosidl_typesupport_fastrtps_c/CMakeLists.txt b/rosidl_typesupport_fastrtps_c/CMakeLists.txt index 8cc8cf2..7181915 100644 --- a/rosidl_typesupport_fastrtps_c/CMakeLists.txt +++ b/rosidl_typesupport_fastrtps_c/CMakeLists.txt @@ -35,8 +35,10 @@ endif() find_package(ament_cmake_python REQUIRED) find_package(rosidl_runtime_c REQUIRED) +find_package(fastcdr REQUIRED CONFIG) ament_export_dependencies(rosidl_runtime_c) +ament_export_dependencies(fastcdr) ament_python_install_package(${PROJECT_NAME}) @@ -49,6 +51,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") target_link_libraries(${PROJECT_NAME} PUBLIC + fastcdr rosidl_runtime_c::rosidl_runtime_c) # Export old-style CMake variables diff --git a/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/serialization_helpers.hpp b/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/serialization_helpers.hpp new file mode 100644 index 0000000..84e4de8 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/serialization_helpers.hpp @@ -0,0 +1,81 @@ +// Copyright 2024 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSIDL_TYPESUPPORT_FASTRTPS_C__SERIALIZATION_HELPERS_HPP_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_C__SERIALIZATION_HELPERS_HPP_ + +#include +#include +#include + +#include "fastcdr/Cdr.h" +#include "fastcdr/exceptions/BadParamException.h" + +#include "rosidl_runtime_c/u16string.h" +#include "rosidl_runtime_c/u16string_functions.h" + +#include "rosidl_typesupport_fastrtps_c/visibility_control.h" + +namespace rosidl_typesupport_fastrtps_c +{ + +inline void cdr_serialize( + eprosima::fastcdr::Cdr & cdr, + const rosidl_runtime_c__U16String & u16str) +{ + if (u16str.size > std::numeric_limits::max()) { + throw std::overflow_error("String length exceeds does not fit in CDR serialization format"); + } + auto len = static_cast(u16str.size); + cdr << len; + for (uint32_t i = 0; i < len; ++i) { + // We are serializing a uint32_t for interoperability with other DDS-based implementations. + // We might change this to a uint16_t in the future if we don't mind breaking backward + // compatibility and we make the change for all the supported DDS implementations at the same + // time. + uint32_t c = static_cast(u16str.data[i]); + cdr << c; + } +} + +inline bool cdr_deserialize( + eprosima::fastcdr::Cdr & cdr, + rosidl_runtime_c__U16String & u16str) +{ + uint32_t len; + cdr >> len; + bool succeeded = rosidl_runtime_c__U16String__resize(&u16str, len); + if (!succeeded) { + return false; + } + + for (uint32_t i = 0; i < len; ++i) { + // We are serializing a uint32_t for interoperability with other DDS-based implementations. + // If we change this to a uint16_t in the future, we could remove the check below, since all + // serialized values would fit in the destination type. + uint32_t c; + cdr >> c; + if (c > std::numeric_limits::max()) { + throw eprosima::fastcdr::exception::BadParamException( + "Character value exceeds maximum value"); + } + u16str.data[i] = static_cast(c); + } + + return true; +} + +} // namespace rosidl_typesupport_fastrtps_c + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_C__SERIALIZATION_HELPERS_HPP_ diff --git a/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/wstring_conversion.hpp b/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/wstring_conversion.hpp index ce85914..b7a3f35 100644 --- a/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/wstring_conversion.hpp +++ b/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/wstring_conversion.hpp @@ -17,6 +17,7 @@ #include +#include "rcutils/macros.h" #include "rosidl_runtime_c/u16string.h" #include "rosidl_typesupport_fastrtps_c/visibility_control.h" @@ -29,6 +30,7 @@ namespace rosidl_typesupport_fastrtps_c * \param[in,out] wstr The std::wstring to copy to. */ ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC + RCUTILS_DEPRECATED_WITH_MSG("not used by core packages") void u16string_to_wstring( const rosidl_runtime_c__U16String & u16str, std::wstring & wstr); @@ -39,6 +41,7 @@ void u16string_to_wstring( * \return true if resizing u16str and assignment succeeded, otherwise false. */ ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC + RCUTILS_DEPRECATED_WITH_MSG("not used by core packages") bool wstring_to_u16string( const std::wstring & wstr, rosidl_runtime_c__U16String & u16str); 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..de51c67 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 @@ -32,6 +32,7 @@ header_files = [ 'string', # Provides the rosidl_typesupport_fastrtps_c__identifier symbol declaration. 'rosidl_typesupport_fastrtps_c/identifier.h', + 'rosidl_typesupport_fastrtps_c/serialization_helpers.hpp', 'rosidl_typesupport_fastrtps_c/wstring_conversion.hpp', # Provides the definition of the message_type_support_callbacks_t struct. 'rosidl_typesupport_fastrtps_cpp/message_type_support.h', @@ -215,7 +216,6 @@ if isinstance(type_, AbstractNestedType): 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) { @@ -226,8 +226,7 @@ if isinstance(type_, AbstractNestedType): fprintf(stderr, "string not null-terminated\n"); return false; } - rosidl_typesupport_fastrtps_c::u16string_to_wstring(*str, wstr); - cdr << wstr; + rosidl_typesupport_fastrtps_c::cdr_serialize(cdr, *str); } @[ elif isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == 'wchar']@ for (size_t i = 0; i < size; ++i) { @@ -260,9 +259,7 @@ if isinstance(type_, AbstractNestedType): } 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; + rosidl_typesupport_fastrtps_c::cdr_serialize(cdr, ros_message->@(member.name)); @[ elif isinstance(member.type, BasicType) and member.type.typename == 'boolean']@ cdr << (ros_message->@(member.name) ? true : false); @[ elif isinstance(member.type, BasicType) and member.type.typename == 'wchar']@ @@ -362,8 +359,7 @@ else: if (!ros_i.data) { rosidl_runtime_c__U16String__init(&ros_i); } - cdr >> wstr; - bool succeeded = rosidl_typesupport_fastrtps_c::wstring_to_u16string(wstr, ros_i); + bool succeeded = rosidl_typesupport_fastrtps_c::cdr_deserialize(cdr, ros_i); if (!succeeded) { fprintf(stderr, "failed to create wstring from u16string\n"); rosidl_runtime_c__U16String__fini(&ros_i); @@ -410,9 +406,7 @@ else: if (!ros_message->@(member.name).data) { rosidl_runtime_c__U16String__init(&ros_message->@(member.name)); } - std::wstring wstr; - cdr >> wstr; - bool succeeded = rosidl_typesupport_fastrtps_c::wstring_to_u16string(wstr, ros_message->@(member.name)); + bool succeeded = rosidl_typesupport_fastrtps_c::cdr_deserialize(cdr, ros_message->@(member.name)); if (!succeeded) { fprintf(stderr, "failed to create wstring from u16string\n"); rosidl_runtime_c__U16String__fini(&ros_message->@(member.name)); diff --git a/rosidl_typesupport_fastrtps_c/test/benchmark/benchmark_string_conversions.cpp b/rosidl_typesupport_fastrtps_c/test/benchmark/benchmark_string_conversions.cpp index c6a3cbe..175ac64 100644 --- a/rosidl_typesupport_fastrtps_c/test/benchmark/benchmark_string_conversions.cpp +++ b/rosidl_typesupport_fastrtps_c/test/benchmark/benchmark_string_conversions.cpp @@ -14,12 +14,13 @@ #include +#include "fastcdr/Cdr.h" #include "rcutils/macros.h" #include "rosidl_runtime_c/string_functions.h" #include "rosidl_runtime_c/u16string_functions.h" -#include "rosidl_typesupport_fastrtps_c/wstring_conversion.hpp" +#include "rosidl_typesupport_fastrtps_c/serialization_helpers.hpp" #include "performance_test_fixture/performance_test_fixture.hpp" @@ -32,7 +33,13 @@ constexpr const uint64_t kSize = 1024; BENCHMARK_F(PerformanceTest, wstring_to_u16string)(benchmark::State & st) { + namespace fastcdr = eprosima::fastcdr; + std::wstring wstring(kSize, '*'); + char raw_buffer[kSize * 4 + 4]; // 4 bytes per character + 4 bytes for the length + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr cdr(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); + cdr << wstring; rosidl_runtime_c__U16String s; if (!rosidl_runtime_c__U16String__init(&s)) { @@ -44,7 +51,8 @@ BENCHMARK_F(PerformanceTest, wstring_to_u16string)(benchmark::State & st) for (auto _ : st) { RCUTILS_UNUSED(_); - rosidl_typesupport_fastrtps_c::wstring_to_u16string(wstring, s); + cdr.reset(); + rosidl_typesupport_fastrtps_c::cdr_deserialize(cdr, s); } rosidl_runtime_c__U16String__fini(&s); @@ -52,7 +60,13 @@ BENCHMARK_F(PerformanceTest, wstring_to_u16string)(benchmark::State & st) BENCHMARK_F(PerformanceTest, u16string_to_wstring)(benchmark::State & st) { + namespace fastcdr = eprosima::fastcdr; + std::wstring data(kSize, '*'); + char raw_buffer[kSize * 4 + 4]; // 4 bytes per character + 4 bytes for the length + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr cdr(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); + cdr << data; rosidl_runtime_c__U16String s; if (!rosidl_runtime_c__U16String__init(&s)) { @@ -61,14 +75,15 @@ BENCHMARK_F(PerformanceTest, u16string_to_wstring)(benchmark::State & st) } // Just do a copy - rosidl_typesupport_fastrtps_c::wstring_to_u16string(data, s); + cdr.reset(); + rosidl_typesupport_fastrtps_c::cdr_deserialize(cdr, s); reset_heap_counters(); for (auto _ : st) { RCUTILS_UNUSED(_); - std::wstring actual; - rosidl_typesupport_fastrtps_c::u16string_to_wstring(s, actual); + cdr.reset(); + rosidl_typesupport_fastrtps_c::cdr_serialize(cdr, s); } rosidl_runtime_c__U16String__fini(&s); diff --git a/rosidl_typesupport_fastrtps_c/test/test_wstring_conversion.cpp b/rosidl_typesupport_fastrtps_c/test/test_wstring_conversion.cpp index b451f43..fb3aced 100644 --- a/rosidl_typesupport_fastrtps_c/test/test_wstring_conversion.cpp +++ b/rosidl_typesupport_fastrtps_c/test/test_wstring_conversion.cpp @@ -16,37 +16,43 @@ #include "gtest/gtest.h" +#include "fastcdr/Cdr.h" #include "rosidl_runtime_c/u16string_functions.h" #include "osrf_testing_tools_cpp/scope_exit.hpp" -#include "rosidl_typesupport_fastrtps_c/wstring_conversion.hpp" +#include "rosidl_typesupport_fastrtps_c/serialization_helpers.hpp" -using rosidl_typesupport_fastrtps_c::u16string_to_wstring; -using rosidl_typesupport_fastrtps_c::wstring_to_u16string; +using rosidl_typesupport_fastrtps_c::cdr_serialize; +using rosidl_typesupport_fastrtps_c::cdr_deserialize; -TEST(test_wstring_conversion, wstring_to_u16string) +void test_ser_des(const rosidl_runtime_c__U16String & input) { - rosidl_runtime_c__U16String actual; - ASSERT_TRUE(rosidl_runtime_c__U16String__init(&actual)); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + namespace fastcdr = eprosima::fastcdr; + char raw_buffer[1024]; + { - rosidl_runtime_c__U16String__fini(&actual); - }); + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr serializer(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); - // Default string - EXPECT_TRUE(wstring_to_u16string(std::wstring(), actual)); - EXPECT_EQ(0, memcmp(u"", actual.data, actual.size)); + cdr_serialize(serializer, input); + } - // Empty string - EXPECT_TRUE(wstring_to_u16string(std::wstring(L""), actual)); - EXPECT_EQ(0, memcmp(u"", actual.data, actual.size)); + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr deserializer(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); - // Non-empty string - EXPECT_TRUE(wstring_to_u16string(std::wstring(L"¡Hola, Mundo!"), actual)); - EXPECT_EQ(0, memcmp(u"¡Hola, Mundo!", actual.data, actual.size)); + rosidl_runtime_c__U16String output; + ASSERT_TRUE(rosidl_runtime_c__U16String__init(&output)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rosidl_runtime_c__U16String__fini(&output); + }); + + ASSERT_TRUE(cdr_deserialize(deserializer, output)); + EXPECT_EQ(input.size, output.size); + EXPECT_EQ(0, memcmp(input.data, output.data, input.size)); } -TEST(test_wstring_conversion, u16string_to_wstring) +TEST(test_wstring_conversion, serialize_deserialize) { std::wstring actual; rosidl_runtime_c__U16String input; @@ -57,16 +63,13 @@ TEST(test_wstring_conversion, u16string_to_wstring) }); // Default string - u16string_to_wstring(input, actual); - EXPECT_EQ(std::wstring(), actual); + test_ser_des(input); // Empty string ASSERT_TRUE(rosidl_runtime_c__U16String__assign(&input, (const uint16_t *)u"")); - u16string_to_wstring(input, actual); - EXPECT_EQ(std::wstring(L""), actual); + test_ser_des(input); // Non-empty string ASSERT_TRUE(rosidl_runtime_c__U16String__assign(&input, (const uint16_t *)u"¡Hola, Mundo!")); - u16string_to_wstring(input, actual); - EXPECT_EQ(std::wstring(L"¡Hola, Mundo!"), actual); + test_ser_des(input); } diff --git a/rosidl_typesupport_fastrtps_c/test/test_wstring_conversion_mem.cpp b/rosidl_typesupport_fastrtps_c/test/test_wstring_conversion_mem.cpp index 66b7761..a18d3b2 100644 --- a/rosidl_typesupport_fastrtps_c/test/test_wstring_conversion_mem.cpp +++ b/rosidl_typesupport_fastrtps_c/test/test_wstring_conversion_mem.cpp @@ -16,16 +16,19 @@ #include "gtest/gtest.h" +#include "fastcdr/Cdr.h" #include "rosidl_runtime_c/u16string_functions.h" #include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" #include "osrf_testing_tools_cpp/scope_exit.hpp" -#include "rosidl_typesupport_fastrtps_c/wstring_conversion.hpp" +#include "rosidl_typesupport_fastrtps_c/serialization_helpers.hpp" -using rosidl_typesupport_fastrtps_c::wstring_to_u16string; +using rosidl_typesupport_fastrtps_c::cdr_deserialize; TEST(test_wstring_conversion, u16string_resize_failure) { + namespace fastcdr = eprosima::fastcdr; + osrf_testing_tools_cpp::memory_tools::initialize(); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { @@ -33,6 +36,13 @@ TEST(test_wstring_conversion, u16string_resize_failure) }); std::wstring wstring(L"¡Hola, Mundo!"); + char raw_buffer[1024]; + { + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr cdr(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); + cdr << wstring; + } + rosidl_runtime_c__U16String u16string; ASSERT_TRUE(rosidl_runtime_c__U16String__init(&u16string)); @@ -50,8 +60,10 @@ TEST(test_wstring_conversion, u16string_resize_failure) osrf_testing_tools_cpp::memory_tools::on_unexpected_realloc(on_unexpected_operation); osrf_testing_tools_cpp::memory_tools::enable_monitoring(); + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr cdr(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); EXPECT_NO_MEMORY_OPERATIONS( { - EXPECT_FALSE(wstring_to_u16string(wstring, u16string)); + EXPECT_FALSE(cdr_deserialize(cdr, u16string)); }); } diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp new file mode 100644 index 0000000..0b70af5 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp @@ -0,0 +1,78 @@ +// Copyright 2024 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERIALIZATION_HELPERS_HPP_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERIALIZATION_HELPERS_HPP_ + +#include + +#include +#include + +#include +#include +#include + +namespace rosidl_typesupport_fastrtps_cpp +{ + +inline void cdr_serialize( + eprosima::fastcdr::Cdr & cdr, + const std::u16string & u16str) +{ + if (u16str.size() > std::numeric_limits::max()) { + throw std::overflow_error("String length exceeds does not fit in CDR serialization format"); + } + auto len = static_cast(u16str.size()); + cdr << len; + for (uint32_t i = 0; i < len; ++i) { + // We are serializing a uint32_t for interoperability with other DDS-based implementations. + // We might change this to a uint16_t in the future if we don't mind breaking backward + // compatibility and we make the change for all the supported DDS implementations at the same + // time. + uint32_t c = static_cast(u16str[i]); + cdr << c; + } +} + +inline bool cdr_deserialize( + eprosima::fastcdr::Cdr & cdr, + std::u16string & u16str) +{ + uint32_t len; + cdr >> len; + try { + u16str.resize(len); + } catch (...) { + return false; + } + for (uint32_t i = 0; i < len; ++i) { + // We are serializing a uint32_t for interoperability with other DDS-based implementations. + // If we change this to a uint16_t in the future, we could remove the check below, since all + // serialized values would fit in the destination type. + uint32_t c; + cdr >> c; + if (c > std::numeric_limits::max()) { + throw eprosima::fastcdr::exception::BadParamException( + "Character value exceeds maximum value"); + } + u16str[i] = static_cast(c); + } + + return true; +} + +} // namespace rosidl_typesupport_fastrtps_cpp + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERIALIZATION_HELPERS_HPP_ diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp index 35bc7c3..b8dd9a4 100644 --- a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp @@ -19,6 +19,8 @@ #include +#include "rcutils/macros.h" + namespace rosidl_typesupport_fastrtps_cpp { @@ -28,6 +30,7 @@ namespace rosidl_typesupport_fastrtps_cpp * \param[in,out] wstr The std::wstring to copy to. */ ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC + RCUTILS_DEPRECATED_WITH_MSG("not used by core packages") void u16string_to_wstring(const std::u16string & u16str, std::wstring & wstr); /// Convert a std::wstring into a std::u16string. @@ -37,6 +40,7 @@ void u16string_to_wstring(const std::u16string & u16str, std::wstring & wstr); * \return true if resizing u16str and assignment succeeded, otherwise false. */ ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC + RCUTILS_DEPRECATED_WITH_MSG("not used by core packages") bool wstring_to_u16string(const std::wstring & wstr, std::u16string & u16str); } // namespace rosidl_typesupport_fastrtps_cpp 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..e67e787 100644 --- a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -21,6 +21,7 @@ header_files = [ 'rosidl_typesupport_fastrtps_cpp/identifier.hpp', 'rosidl_typesupport_fastrtps_cpp/message_type_support.h', 'rosidl_typesupport_fastrtps_cpp/message_type_support_decl.hpp', + 'rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp', 'rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp', 'fastcdr/Cdr.h', ] @@ -105,17 +106,13 @@ cdr_serialize( @[ 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; + rosidl_typesupport_fastrtps_cpp::cdr_serialize(cdr, ros_message.@(member.name)[i]); @[ end if]@ } @[ end if]@ @@ -137,17 +134,13 @@ cdr_serialize( 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; + rosidl_typesupport_fastrtps_cpp::cdr_serialize(cdr, ros_message.@(member.name)[i]); @[ elif not isinstance(member.type.value_type, NamespacedType)]@ cdr << ros_message.@(member.name)[i]; @[ else]@ @@ -166,9 +159,7 @@ cdr_serialize( 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; + rosidl_typesupport_fastrtps_cpp::cdr_serialize(cdr, ros_message.@(member.name)); } @[ elif not isinstance(member.type, NamespacedType)]@ cdr << ros_message.@(member.name); @@ -195,19 +186,15 @@ cdr_deserialize( @[ 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_deserialize( cdr, ros_message.@(member.name)[i]); @[ else]@ - cdr >> wstr; - bool succeeded = rosidl_typesupport_fastrtps_cpp::wstring_to_u16string(wstr, ros_message.@(member.name)[i]); + bool succeeded = rosidl_typesupport_fastrtps_cpp::cdr_deserialize(cdr, ros_message.@(member.name)[i]); if (!succeeded) { - fprintf(stderr, "failed to create wstring from u16string\n"); + fprintf(stderr, "failed to deserialize u16string\n"); return false; } @[ end if]@ @@ -226,9 +213,6 @@ cdr_deserialize( cdr.deserializeArray(&(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']@ uint8_t tmp; @@ -239,10 +223,9 @@ cdr_deserialize( cdr >> tmp; ros_message.@(member.name)[i] = static_cast(tmp); @[ elif isinstance(member.type.value_type, AbstractWString)]@ - cdr >> wstr; - bool succeeded = rosidl_typesupport_fastrtps_cpp::wstring_to_u16string(wstr, ros_message.@(member.name)[i]); + bool succeeded = rosidl_typesupport_fastrtps_cpp::cdr_deserialize(cdr, ros_message.@(member.name)[i]); if (!succeeded) { - fprintf(stderr, "failed to create wstring from u16string\n"); + fprintf(stderr, "failed to deserialize u16string\n"); return false; } @[ elif not isinstance(member.type.value_type, NamespacedType)]@ @@ -270,11 +253,9 @@ cdr_deserialize( } @[ elif isinstance(member.type, AbstractWString)]@ { - std::wstring wstr; - cdr >> wstr; - bool succeeded = rosidl_typesupport_fastrtps_cpp::wstring_to_u16string(wstr, ros_message.@(member.name)); + bool succeeded = rosidl_typesupport_fastrtps_cpp::cdr_deserialize(cdr, ros_message.@(member.name)); if (!succeeded) { - fprintf(stderr, "failed to create wstring from u16string\n"); + fprintf(stderr, "failed to deserialize u16string\n"); return false; } } diff --git a/rosidl_typesupport_fastrtps_cpp/test/benchmark/benchmark_string_conversions.cpp b/rosidl_typesupport_fastrtps_cpp/test/benchmark/benchmark_string_conversions.cpp index f6148a3..a4ec4cd 100644 --- a/rosidl_typesupport_fastrtps_cpp/test/benchmark/benchmark_string_conversions.cpp +++ b/rosidl_typesupport_fastrtps_cpp/test/benchmark/benchmark_string_conversions.cpp @@ -14,9 +14,10 @@ #include +#include "fastcdr/Cdr.h" #include "rcutils/macros.h" -#include "rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp" +#include "rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp" #include "performance_test_fixture/performance_test_fixture.hpp" @@ -29,26 +30,38 @@ constexpr const uint64_t kSize = 1024; BENCHMARK_F(PerformanceTest, wstring_to_u16string)(benchmark::State & st) { + namespace fastcdr = eprosima::fastcdr; + std::wstring wstring(kSize, '*'); + char raw_buffer[kSize * 4 + 4]; // 4 bytes per character + 4 bytes for the length + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr cdr(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); + cdr << wstring; reset_heap_counters(); for (auto _ : st) { RCUTILS_UNUSED(_); std::u16string u16string; - rosidl_typesupport_fastrtps_cpp::wstring_to_u16string(wstring, u16string); + cdr.reset(); + rosidl_typesupport_fastrtps_cpp::cdr_deserialize(cdr, u16string); } } BENCHMARK_F(PerformanceTest, u16string_to_wstring)(benchmark::State & st) { + namespace fastcdr = eprosima::fastcdr; + std::u16string u16string(kSize, '*'); + char raw_buffer[kSize * 4 + 4]; // 4 bytes per character + 4 bytes for the length + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr cdr(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); reset_heap_counters(); for (auto _ : st) { RCUTILS_UNUSED(_); - std::wstring wstring; - rosidl_typesupport_fastrtps_cpp::u16string_to_wstring(u16string, wstring); + cdr.reset(); + rosidl_typesupport_fastrtps_cpp::cdr_serialize(cdr, u16string); } } diff --git a/rosidl_typesupport_fastrtps_cpp/test/test_wstring_conversion.cpp b/rosidl_typesupport_fastrtps_cpp/test/test_wstring_conversion.cpp index 3275180..2f344b0 100644 --- a/rosidl_typesupport_fastrtps_cpp/test/test_wstring_conversion.cpp +++ b/rosidl_typesupport_fastrtps_cpp/test/test_wstring_conversion.cpp @@ -16,43 +16,42 @@ #include "gtest/gtest.h" +#include "fastcdr/Cdr.h" #include "osrf_testing_tools_cpp/scope_exit.hpp" -#include "rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp" +#include "rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp" -using rosidl_typesupport_fastrtps_cpp::u16string_to_wstring; -using rosidl_typesupport_fastrtps_cpp::wstring_to_u16string; +using rosidl_typesupport_fastrtps_cpp::cdr_serialize; +using rosidl_typesupport_fastrtps_cpp::cdr_deserialize; -TEST(test_wstring_conversion, wstring_to_u16string) +void test_ser_des(const std::u16string & input) { - std::u16string actual; + namespace fastcdr = eprosima::fastcdr; + char raw_buffer[1024]; - // Default string - EXPECT_TRUE(wstring_to_u16string(std::wstring(), actual)); - EXPECT_EQ(std::u16string(), actual); + { + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr serializer(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); - // Empty string - EXPECT_TRUE(wstring_to_u16string(std::wstring(L""), actual)); - EXPECT_EQ(std::u16string(u""), actual); + cdr_serialize(serializer, input); + } - // Non-empty string - EXPECT_TRUE(wstring_to_u16string(std::wstring(L"¡Hola, Mundo!"), actual)); - EXPECT_EQ(std::u16string(u"¡Hola, Mundo!"), actual); + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr deserializer(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); + + std::u16string output; + ASSERT_TRUE(cdr_deserialize(deserializer, output)); + EXPECT_EQ(input, output); } -TEST(test_wstring_conversion, u16string_to_wstring) +TEST(test_wstring_conversion, serialize_deserialize) { - std::wstring actual; - // Default string - u16string_to_wstring(std::u16string(), actual); - EXPECT_EQ(std::wstring(), actual); + test_ser_des(std::u16string()); // Empty string - u16string_to_wstring(std::u16string(u""), actual); - EXPECT_EQ(std::wstring(L""), actual); + test_ser_des(std::u16string(u"")); // Non-empty string - u16string_to_wstring(std::u16string(u"¡Hola, Mundo!"), actual); - EXPECT_EQ(std::wstring(L"¡Hola, Mundo!"), actual); + test_ser_des(std::u16string(u"¡Hola, Mundo!")); } diff --git a/rosidl_typesupport_fastrtps_cpp/test/test_wstring_conversion_mem.cpp b/rosidl_typesupport_fastrtps_cpp/test/test_wstring_conversion_mem.cpp index b53542a..7bdf100 100644 --- a/rosidl_typesupport_fastrtps_cpp/test/test_wstring_conversion_mem.cpp +++ b/rosidl_typesupport_fastrtps_cpp/test/test_wstring_conversion_mem.cpp @@ -16,15 +16,18 @@ #include "gtest/gtest.h" +#include "fastcdr/Cdr.h" #include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" #include "osrf_testing_tools_cpp/scope_exit.hpp" -#include "rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp" +#include "rosidl_typesupport_fastrtps_cpp/serialization_helpers.hpp" -using rosidl_typesupport_fastrtps_cpp::wstring_to_u16string; +using rosidl_typesupport_fastrtps_cpp::cdr_deserialize; TEST(test_wstring_conversion, u16string_resize_failure) { + namespace fastcdr = eprosima::fastcdr; + osrf_testing_tools_cpp::memory_tools::initialize(); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { @@ -32,6 +35,13 @@ TEST(test_wstring_conversion, u16string_resize_failure) }); std::wstring wstring(L"¡Hola, Mundo!"); + char raw_buffer[1024]; + { + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr cdr(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); + cdr << wstring; + } + std::u16string u16string(1, 1); // Force malloc/realloc to fail @@ -43,8 +53,10 @@ TEST(test_wstring_conversion, u16string_resize_failure) osrf_testing_tools_cpp::memory_tools::on_unexpected_realloc(on_unexpected_operation); osrf_testing_tools_cpp::memory_tools::enable_monitoring(); + fastcdr::FastBuffer buffer(raw_buffer, sizeof(raw_buffer)); + fastcdr::Cdr cdr(buffer, fastcdr::Cdr::DEFAULT_ENDIAN, fastcdr::Cdr::DDS_CDR); EXPECT_NO_MEMORY_OPERATIONS( { - EXPECT_FALSE(wstring_to_u16string(wstring, u16string)); + EXPECT_FALSE(cdr_deserialize(cdr, u16string)); }); }