Diff: mros2_msgs/std_msgs/msg/string.hpp

Revision:

2:159877d749c2

Parent:

0:580aba13d1a1

--- a/mros2_msgs/std_msgs/msg/string.hpp	Wed Feb 23 21:37:44 2022 +0900
+++ b/mros2_msgs/std_msgs/msg/string.hpp	Sun Mar 13 21:20:03 2022 +0900
@@ -9,29 +9,45 @@
 public:
   std::string getTypeName();
   std::string data;
+  uint8_t cntPub = 0;
+  uint32_t pubSize;
+  uint32_t subSize;
   void copyToBuf(uint8_t *addrPtr)
   {
-    uint32_t size = data.size();
-    memcpy(addrPtr, &size, 4);
+    pubSize = data.size();
+    memcpy(addrPtr, &pubSize, 4);
     addrPtr += 4;
-    memcpy(addrPtr, data.c_str(),size);
-    addrPtr += size;
-    *addrPtr = 0;
+    cntPub += 4 ;
+    memcpy(addrPtr, data.c_str(),pubSize);
+    addrPtr += pubSize;
+    cntPub += pubSize;  
+    
   }
 
   void copyFromBuf(const uint8_t *addrPtr)
   {
-    uint32_t msg_size;
-    memcpy(&msg_size, addrPtr, 4);
+    memcpy(&subSize, addrPtr, 4);
     addrPtr += 4;
-    data.resize(msg_size);
-    memcpy(&data[0], addrPtr, msg_size);
+    data.resize(subSize);
+    memcpy(&data[0], addrPtr, subSize);
+
+  }
 
+   void memAlign(uint8_t *addrPtr){
+    if (cntPub%4 > 0){
+      addrPtr += cntPub;
+      for(int i=0; i<(4-(cntPub%4)) ; i++){
+        *addrPtr = 0;
+        addrPtr += 1;
+      }
+      cntPub += 4-(cntPub%4);
+    }
+    return;
   }
 
   uint8_t getTotalSize()
   {
-    return (5 + data.size());
+    return cntPub;
   }
 private:
   std::string type_name = "std_msgs::msg::dds_::String";