From 536c3c5e647e5c574d07bfffa7279f010ebdf02f Mon Sep 17 00:00:00 2001
From: Olivier Kermorgant <olivier_kermorgant@yahoo.fr>
Date: Fri, 7 Jul 2023 12:47:00 +0200
Subject: [PATCH] handle special case of std::vector<bool>

---
 resource/interface_factories.cpp.em | 35 +++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/resource/interface_factories.cpp.em b/resource/interface_factories.cpp.em
index f3e536e4..ffc081f9 100644
--- a/resource/interface_factories.cpp.em
+++ b/resource/interface_factories.cpp.em
@@ -368,6 +368,18 @@ static void streamPrimitiveVector(ros::serialization::OStream & stream, const VE
   memcpy(stream.advance(data_len), &vec.front(), data_len);
 }
 
+// This version is for write vector<bool>
+template<typename VEC_PRIMITIVE_T>
+static void streamPrimitiveVectorBool(ros::serialization::OStream & stream, const VEC_PRIMITIVE_T& vec)
+{
+  const uint32_t step = sizeof(bool);
+  const uint32_t data_len = vec.size() * sizeof(bool);
+  // element-wise copy because of vector<bool>
+  for(uint i = 0; i < vec.size(); ++i)
+    *(stream.getData()+i*step) = vec[i];
+  stream.advance(data_len);
+}
+
 // This version is for length
 template<typename VEC_PRIMITIVE_T>
 static void streamPrimitiveVector(ros::serialization::LStream & stream, const VEC_PRIMITIVE_T& vec)
@@ -376,6 +388,14 @@ static void streamPrimitiveVector(ros::serialization::LStream & stream, const VE
   stream.advance(data_len);
 }
 
+// This version is for length
+template<typename VEC_PRIMITIVE_T>
+static void streamPrimitiveVectorBool(ros::serialization::LStream & stream, const VEC_PRIMITIVE_T& vec)
+{
+  const uint32_t data_len = vec.size() * sizeof(typename VEC_PRIMITIVE_T::value_type);
+  stream.advance(data_len);
+}
+
 // This version is for read
 template<typename VEC_PRIMITIVE_T>
 static void streamPrimitiveVector(ros::serialization::IStream & stream, VEC_PRIMITIVE_T& vec)
@@ -385,6 +405,18 @@ static void streamPrimitiveVector(ros::serialization::IStream & stream, VEC_PRIM
   memcpy(&vec.front(), stream.advance(data_len), data_len);
 }
 
+// This version is for read sector<bool>
+template<typename VEC_PRIMITIVE_T>
+static void streamPrimitiveVectorBool(ros::serialization::IStream & stream, VEC_PRIMITIVE_T& vec)
+{
+  const uint32_t step = sizeof(bool);
+  const uint32_t data_len = vec.size() * sizeof(bool);
+  // element-wise copy because of vector<bool>
+  for(uint i = 0; i < vec.size(); ++i)
+    vec[i] = *(stream.getData() + i*step);
+  stream.advance(data_len);
+}
+
 @[for m in mapped_msgs]@
 
 @[  if m.ros2_msg.package_name=="std_msgs" and m.ros2_msg.message_name=="Header"]
@@ -466,6 +498,9 @@ if isinstance(ros2_fields[-1].type, NamespacedType):
   {
     ros1_bridge::internal_stream_translate_helper(stream, *ros2_it);
   }
+@[            elif ros2_fields[-1].type.value_type.typename == 'boolean']@
+  // write primitive type, specialized
+  streamPrimitiveVectorBool(stream, ros2_msg.@(ros2_field_selection));
 @[            else]@
   // write primitive type
   streamPrimitiveVector(stream, ros2_msg.@(ros2_field_selection));