Feature/mros2 frag msg proto

.gitmodules

@@ -1,4 +1,4 @@
 [submodule "embeddedRTPS"]
 	path = embeddedRTPS
-	url = https://github.com/mROS-base/embeddedRTPS.git
-	branch = mros2
+	url = https://github.com/smoriemb/embeddedRTPS.git
+	branch = feature/mros2-frag-msg-proto

mros2_header_generator/header_template.tpl

@@ -40,6 +40,65 @@ class {{msg.name}}
 public:
   uint32_t cntPub = 0;
   uint32_t cntSub = 0;
+  uint32_t idxSerialized = 0;
+
+  typedef std::pair<bool, uint32_t> FragCopyReturnType;
+
+  template <class T>
+  uint32_t copyPrimToFragBufLocal(uint8_t*& addrPtr,
+                                  const uint32_t cntPub,
+                                  const uint32_t size,
+                                  const T& data)
+  {
+    uint32_t lenPad = (0 == (cntPub % sizeof(T))) ?
+      0 : (sizeof(T) - (cntPub % sizeof(T))); // this doesn't get along with float128.
+    if ( size < sizeof(T) ) {
+      // There are no enough space.
+      return 0;
+    }
+    // Put padding space
+    for(int i = 0; i < lenPad; i++){
+      *addrPtr = 0;
+      addrPtr += 1;
+    }
+    // Store serialzed value.
+    memcpy(addrPtr, &data, sizeof(T));
+    addrPtr += sizeof(T);
+
+    return sizeof(T) + lenPad;
+  }
+
+  template<class T>
+  FragCopyReturnType copyArrayToFragBufLocal(uint8_t*& addrPtr,
+                                             const uint32_t size,
+                                             T& data,
+                                             uint32_t& cntPubMemberLocal)
+  {
+    uint32_t pubDataSize = data.size();
+    uint32_t cntLocalFrag = 0;
+
+    if (cntPubMemberLocal < sizeof(uint32_t)) {
+      if (size < sizeof(uint32_t)) {
+        return {false, 0};
+      }
+      memcpy(addrPtr, &pubDataSize, sizeof(uint32_t));
+      addrPtr += sizeof(uint32_t);
+      cntPubMemberLocal += sizeof(uint32_t);
+      cntLocalFrag += sizeof(uint32_t);
+    }
+
+    uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t));
+                       // cntPubMemberLocal > 4 here
+    uint32_t tmp = std::min(pubDataSize - cntFrag, size - cntLocalFrag);
+    if (0 < tmp) {
+      memcpy(addrPtr, data.data() + cntFrag, tmp);
+      addrPtr += tmp;
+      cntPubMemberLocal += tmp;
+      cntLocalFrag += tmp;
+    }
+
+    return {(cntPubMemberLocal - sizeof(uint32_t)) >= pubDataSize, cntLocalFrag};
+  }
 
   {%for def_data in msg.def %}  
   {%if def_data.boundedArray %}std::array<{{def_data.cppType}}, {{def_data.boundedArray}}>{% elif def_data.isArray %}std::vector<{{def_data.cppType}}>{% elif def_data.cppType == "header" %}int32_t sec; uint32_t nanosec; string frame_id;{% else %}{{def_data.cppType}}{% endif %}{%if def_data.cppType != "header" %} {{def_data.typeName}};{%endif%}
@@ -469,6 +528,41 @@ public:
     return tmpCntPub ;
   }
 
+  uint32_t getPubCnt()
+  {
+    return cntPub;
+  }
+
+  uint32_t calcRawTotalSize()
+  {
+    // TODO: store template code here
+    return 0;
+  }
+
+  uint32_t calcTotalSize()
+  {
+    uint32_t tmp;
+    tmp = 4 // CDR encoding version.
+          + calcRawTotalSize();
+    tmp += ( 0 == (tmp % 4) ? // Padding
+	     0 : (4 - (tmp % 4)) );
+    return tmp;
+  }
+
+  void resetCount()
+  {
+    cntPub = 0;
+    cntSub = 0;
+    idxSerialized = 0;
+    // TODO: store template code here
+    return;
+  }
+
+  FragCopyReturnType  copyToFragBuf(uint8_t *addrPtr, uint32_t size)
+  {
+    // TODO: store template code here
+    return {false, 0};
+  }
 private:
   std::string type_name = "{{msg.pkg}}::msg::dds_::{{msg.name}}";
 };
@@ -486,4 +580,4 @@ struct TypeName<{{msg.pkg}}::msg::{{msg.name}}*> {
 };
 }
 
-#endif
+#endif