Add wstring support

Signed-off-by: Erik Boasson <eb@ilities.com>

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)

rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp

@@ -18,6 +18,7 @@
 
 #include <rosidl_generator_c/string.h>
 #include <rosidl_generator_c/string_functions.h>
+#include <rosidl_generator_c/u16string_functions.h>
 
 #include <cassert>
 #include <string>

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<std::wstring>(
+  const rosidl_typesupport_introspection_cpp::MessageMember * member,
+  void * field,
+  cycser & ser)
+{
+  std::wstring wstr;
+  if (!member->is_array_) {
+    auto u16str = static_cast<std::u16string *>(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<uint32_t>(size);
+    }
+    for (size_t i = 0; i < size; ++i) {
+      const void * element = member->get_const_function(field, i);
+      auto u16str = static_cast<const std::u16string *>(element);
+      u16string_to_wstring(*u16str, wstr);
+      ser << wstr;
+    }
+  }
+}
+
 // C specialization
 template<typename T>
 void serialize_field(
@@ -275,6 +306,34 @@ void serialize_field<std::string>(
   }
 }
 
+template<>
+inline
+void serialize_field<std::wstring>(
+  const rosidl_typesupport_introspection_c__MessageMember * member,
+  void * field,
+  cycser & ser)
+{
+  std::wstring wstr;
+  if (!member->is_array_) {
+    auto u16str = static_cast<rosidl_generator_c__U16String *>(field);
+    u16string_to_wstring(*u16str, wstr);
+    ser << wstr;
+  } else if (member->array_size_ && !member->is_upper_bound_) {
+    auto array = static_cast<rosidl_generator_c__U16String *>(field);
+    for (size_t i = 0; i < member->array_size_; ++i) {
+      u16string_to_wstring(array[i], wstr);
+      ser << wstr;
+    }
+  } else {
+    auto sequence = static_cast<rosidl_generator_c__U16String__Sequence *>(field);
+    ser << static_cast<uint32_t>(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<MembersType>::serializeROSmessage(
       case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING:
         serialize_field<std::string>(member, field, ser);
         break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING:
+        serialize_field<std::wstring>(member, field, ser);
+        break;
       case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE:
         {
           auto sub_members = static_cast<const MembersType *>(member->members_->data);
@@ -453,6 +515,36 @@ inline void deserialize_field<std::string>(
   }
 }
 
+template<>
+inline void deserialize_field<std::wstring>(
+  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<std::u16string *>(field));
+  } else {
+    uint32_t size;
+    if (member->array_size_ && !member->is_upper_bound_) {
+      size = static_cast<uint32_t>(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<std::u16string *>(element);
+      deser >> wstr;
+      wstring_to_u16string(wstr, *u16str);
+    }
+  }
+}
+
 template<typename T>
 void deserialize_field(
   const rosidl_typesupport_introspection_c__MessageMember * member,
@@ -519,6 +611,39 @@ inline void deserialize_field<std::string>(
   }
 }
 
+template<>
+inline void deserialize_field<std::wstring>(
+  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<rosidl_generator_c__U16String *>(field));
+  } else if (member->array_size_ && !member->is_upper_bound_) {
+    auto array = static_cast<rosidl_generator_c__U16String *>(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<rosidl_generator_c__U16String__Sequence *>(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<MembersType>::deserializeROSmessage(
       case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING:
         deserialize_field<std::string>(member, field, deser, call_new);
         break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING:
+        deserialize_field<std::wstring>(member, field, deser, call_new);
+        break;
       case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE:
         {
           auto sub_members = (const MembersType *)member->members_->data;

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<class T>
   inline cycser & operator<<(const std::vector<T> & x) {serialize(x); return *this;}
   template<class T, size_t S>
@@ -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<uint32_t>(sz));
+    resize(off + sz * sizeof(wchar_t));
+    memcpy(data() + off, reinterpret_cast<const char *>(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<class T>
   inline cycdeser & operator>>(std::vector<T> & x) {deserialize(x); return *this;}
   template<class T, size_t S>
@@ -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<const wchar_t *>(data + pos), sz - 1);
+    pos += sz * sizeof(wchar_t);
+  }
 
 #define SIMPLEA(T) inline void deserializeA(T * x, size_t cnt) { \
     if (cnt > 0) { \

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 <string>
+#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_

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 <string>
+#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<wchar_t>(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<char16_t>(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<wchar_t>(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<char16_t>(wstr[i]);
+  }
+  return true;
+}
+
+}  // namespace rmw_cyclonedds_cpp