From 10f18f05251d44076c275b021360657560aaea97 Mon Sep 17 00:00:00 2001 From: Erik Boasson Date: Mon, 19 Aug 2019 11:09:01 +0200 Subject: [PATCH] Add wstring support Signed-off-by: Erik Boasson --- rmw_cyclonedds_cpp/CMakeLists.txt | 1 + .../rmw_cyclonedds_cpp/TypeSupport.hpp | 1 + .../rmw_cyclonedds_cpp/TypeSupport_impl.hpp | 128 ++++++++++++++++++ .../include/rmw_cyclonedds_cpp/serdes.hpp | 16 +++ .../include/rmw_cyclonedds_cpp/u16string.hpp | 36 +++++ rmw_cyclonedds_cpp/src/u16string.cpp | 62 +++++++++ 6 files changed, 244 insertions(+) create mode 100644 rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/u16string.hpp create mode 100644 rmw_cyclonedds_cpp/src/u16string.cpp diff --git a/rmw_cyclonedds_cpp/CMakeLists.txt b/rmw_cyclonedds_cpp/CMakeLists.txt index 369d84b..a3d5f80 100644 --- a/rmw_cyclonedds_cpp/CMakeLists.txt +++ b/rmw_cyclonedds_cpp/CMakeLists.txt @@ -54,6 +54,7 @@ add_library(rmw_cyclonedds_cpp src/rmw_node.cpp src/serdata.cpp src/serdes.cpp + src/u16string.cpp ) target_link_libraries(rmw_cyclonedds_cpp CycloneDDS::ddsc) diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp index 6c8079f..cf2b92b 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp index b1bb25b..3957982 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp @@ -31,8 +31,10 @@ #include "rosidl_typesupport_introspection_c/service_introspection.h" #include "rosidl_generator_c/primitives_sequence_functions.h" +#include "rosidl_generator_c/u16string_functions.h" #include "serdes.hpp" +#include "u16string.hpp" namespace rmw_cyclonedds_cpp { @@ -219,6 +221,35 @@ void serialize_field( } } +template<> +inline +void serialize_field( + const rosidl_typesupport_introspection_cpp::MessageMember * member, + void * field, + cycser & ser) +{ + std::wstring wstr; + if (!member->is_array_) { + auto u16str = static_cast(field); + 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); + u16string_to_wstring(*u16str, wstr); + ser << wstr; + } + } +} + // C specialization template void serialize_field( @@ -275,6 +306,34 @@ void serialize_field( } } +template<> +inline +void serialize_field( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + cycser & ser) +{ + std::wstring wstr; + if (!member->is_array_) { + auto u16str = static_cast(field); + 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) { + 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) { + 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, @@ -363,6 +422,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); @@ -453,6 +515,36 @@ inline void deserialize_field( } } +template<> +inline void deserialize_field( + const rosidl_typesupport_introspection_cpp::MessageMember * member, + void * field, + cycdeser & deser, + bool call_new) +{ + (void)call_new; + std::wstring wstr; + if (!member->is_array_) { + deser >> wstr; + 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; + wstring_to_u16string(wstr, *u16str); + } + } +} + template void deserialize_field( const rosidl_typesupport_introspection_c__MessageMember * member, @@ -519,6 +611,39 @@ inline void deserialize_field( } } +template<> +inline void deserialize_field( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + cycdeser & deser, + bool call_new) +{ + (void)call_new; + std::wstring wstr; + if (!member->is_array_) { + deser >> wstr; + 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; + 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; + wstring_to_u16string(wstr, sequence->data[i]); + } + } +} + inline size_t get_submessage_array_deserialize( const rosidl_typesupport_introspection_cpp::MessageMember * member, cycdeser & deser, @@ -610,6 +735,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; diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp index 4abab01..8018448 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp @@ -42,6 +42,7 @@ public: inline cycser & operator<<(double x) {serialize(x); return *this;} inline cycser & operator<<(const char * x) {serialize(x); return *this;} inline cycser & operator<<(const std::string & x) {serialize(x); return *this;} + inline cycser & operator<<(const std::wstring & x) {serialize(x); return *this;} template inline cycser & operator<<(const std::vector & x) {serialize(x); return *this;} template @@ -88,6 +89,14 @@ public: memcpy(data() + off, x.c_str(), sz); off += sz; } + inline void serialize(const std::wstring & x) + { + size_t sz = x.size() + 1; + serialize(static_cast(sz)); + resize(off + sz * sizeof(wchar_t)); + memcpy(data() + off, reinterpret_cast(x.c_str()), sz * sizeof(wchar_t)); + off += sz * sizeof(wchar_t); + } #define SIMPLEA(T) inline void serializeA(const T * x, size_t cnt) { \ if (cnt > 0) { \ @@ -165,6 +174,7 @@ public: inline cycdeser & operator>>(double & x) {deserialize(x); return *this;} inline cycdeser & operator>>(char * & x) {deserialize(x); return *this;} inline cycdeser & operator>>(std::string & x) {deserialize(x); return *this;} + inline cycdeser & operator>>(std::wstring & x) {deserialize(x); return *this;} template inline cycdeser & operator>>(std::vector & x) {deserialize(x); return *this;} template @@ -209,6 +219,12 @@ public: x = std::string(data + pos, sz - 1); pos += sz; } + inline void deserialize(std::wstring & x) + { + const uint32_t sz = deserialize32(); + x = std::wstring(reinterpret_cast(data + pos), sz - 1); + pos += sz * sizeof(wchar_t); + } #define SIMPLEA(T) inline void deserializeA(T * x, size_t cnt) { \ if (cnt > 0) { \ diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/u16string.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/u16string.hpp new file mode 100644 index 0000000..eae6f09 --- /dev/null +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/u16string.hpp @@ -0,0 +1,36 @@ +// Copyright 2019 ADLINK Technology +// +// 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 RMW_CYCLONEDDS_CPP__U16STRING_HPP_ +#define RMW_CYCLONEDDS_CPP__U16STRING_HPP_ + +#include +#include "rosidl_generator_c/u16string_functions.h" + +namespace rmw_cyclonedds_cpp +{ + +void u16string_to_wstring( + const rosidl_generator_c__U16String & u16str, std::wstring & wstr); + +bool wstring_to_u16string( + const std::wstring & wstr, rosidl_generator_c__U16String & u16str); + +void u16string_to_wstring(const std::u16string & u16str, std::wstring & wstr); + +bool wstring_to_u16string(const std::wstring & wstr, std::u16string & u16str); + +} // namespace rmw_cyclonedds_cpp + +#endif // RMW_CYCLONEDDS_CPP__U16STRING_HPP_ diff --git a/rmw_cyclonedds_cpp/src/u16string.cpp b/rmw_cyclonedds_cpp/src/u16string.cpp new file mode 100644 index 0000000..ec0d454 --- /dev/null +++ b/rmw_cyclonedds_cpp/src/u16string.cpp @@ -0,0 +1,62 @@ +// Copyright 2019 ADLINK Technology +// +// 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. + +#include +#include "rosidl_generator_c/u16string_functions.h" + +namespace rmw_cyclonedds_cpp +{ + +void u16string_to_wstring(const std::u16string & u16str, std::wstring & wstr) +{ + wstr.resize(u16str.size()); + for (size_t i = 0; i < u16str.size(); ++i) { + wstr[i] = static_cast(u16str[i]); + } +} + +bool wstring_to_u16string(const std::wstring & wstr, std::u16string & u16str) +{ + try { + u16str.resize(wstr.size()); + } catch (...) { + return false; + } + for (size_t i = 0; i < wstr.size(); ++i) { + u16str[i] = static_cast(wstr[i]); + } + return true; +} + +void u16string_to_wstring(const rosidl_generator_c__U16String & u16str, std::wstring & wstr) +{ + wstr.resize(u16str.size); + for (size_t i = 0; i < u16str.size; ++i) { + wstr[i] = static_cast(u16str.data[i]); + } +} + +bool wstring_to_u16string(const std::wstring & wstr, rosidl_generator_c__U16String & u16str) +{ + bool succeeded = rosidl_generator_c__U16String__resize(&u16str, wstr.size()); + if (!succeeded) { + return false; + } + for (size_t i = 0; i < wstr.size(); ++i) { + u16str.data[i] = static_cast(wstr[i]); + } + return true; +} + +} // namespace rmw_cyclonedds_cpp