From 189e482df3e1d1370de47c2aaa3b6f5f5dcb0b3c Mon Sep 17 00:00:00 2001 From: takasehideki Date: Thu, 5 Oct 2023 20:31:34 +0900 Subject: [PATCH 1/4] formatten template for custom msgtype header to generate, with BEST_EFFORT,,, (fix #32) --- mros2_header_generator/header_template.tpl | 418 ++++++++++++--------- 1 file changed, 239 insertions(+), 179 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index da53e5d..bd401c9 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -45,19 +45,20 @@ public: typedef std::pair FragCopyReturnType; template - uint32_t copyPrimToFragBufLocal(uint8_t*& addrPtr, + uint32_t copyPrimToFragBufLocal(uint8_t *&addrPtr, const uint32_t cntPub, const uint32_t size, - const T& data) + 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) ) { + 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++){ + for (int i = 0; i < lenPad; i++) + { *addrPtr = 0; addrPtr += 1; } @@ -68,17 +69,19 @@ public: return sizeof(T) + lenPad; } - template - FragCopyReturnType copyArrayToFragBufLocal(uint8_t*& addrPtr, + template + FragCopyReturnType copyArrayToFragBufLocal(uint8_t *&addrPtr, const uint32_t size, - T& data, - uint32_t& cntPubMemberLocal) + T &data, + uint32_t &cntPubMemberLocal) { uint32_t pubDataSize = data.size(); uint32_t cntLocalFrag = 0; - if (cntPubMemberLocal < sizeof(uint32_t)) { - if (size < sizeof(uint32_t)) { + if (cntPubMemberLocal < sizeof(uint32_t)) + { + if (size < sizeof(uint32_t)) + { return {false, 0}; } memcpy(addrPtr, &pubDataSize, sizeof(uint32_t)); @@ -87,10 +90,10 @@ public: cntLocalFrag += sizeof(uint32_t); } - uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); - // cntPubMemberLocal > 4 here + uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); // cntPubMemberLocal > 4 here uint32_t tmp = std::min(pubDataSize - cntFrag, size - cntLocalFrag); - if (0 < tmp) { + if (0 < tmp) + { memcpy(addrPtr, data.data() + cntFrag, tmp); addrPtr += tmp; cntPubMemberLocal += tmp; @@ -100,7 +103,7 @@ public: return {(cntPubMemberLocal - sizeof(uint32_t)) >= pubDataSize, cntLocalFrag}; } - {%for def_data in msg.def %} + {%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%} {% endfor %} @@ -112,25 +115,26 @@ public: {%for def_data in msg.def %} {% if def_data.isCustomType%} {% if def_data.isArray%} + if (cntPub % 4 > 0) { - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ + for (int i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; } - cntPub += 4-(cntPub%4); + cntPub += 4 - (cntPub % 4); } arraySize = {{def_data.typeName}}.size(); - memcpy(addrPtr,&arraySize,4); + memcpy(addrPtr, &arraySize, 4); addrPtr += 4; cntPub += 4; - for(int i=0; i0 && 2 <= (4-(cntPub%4))){ - for (int i=0; i<(4-(cntPub%4))-2; i++){ - *addrPtr = 0; - addrPtr += 1; + if (cntPub % 4 > 0 && 2 <= (4 - (cntPub % 4))) + { + for (int i = 0; i < (4 - (cntPub % 4))-2; i++) + { + *addrPtr = 0; + addrPtr += 1; } - cntPub += (4-(cntPub%4))-2; - } else if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ + cntPub += (4 - (cntPub % 4)) - 2; + } else if (cntPub % 4 > 0) + { + for (int i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 4-(cntPub%4); + } + cntPub += 4 - (cntPub % 4); } {% elif def_data.size==4 %} - if (cntPub%4 > 0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ + if (cntPub % 4 > 0) + { + for (int i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 4-(cntPub%4); + } + cntPub += 4 - (cntPub % 4); } {% elif def_data.size==8 %} - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (int i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } {% endif %} - const {{def_data.cppType}}* ptr = {{def_data.typeName}}.data(); - for(int i=0; i<{{def_data.boundedArray}} ; i++){ - memcpy(addrPtr, &(ptr[i]),{{def_data.size}}); + const {{def_data.cppType}} *ptr = {{def_data.typeName}}.data(); + for (int i = 0; i < {{def_data.boundedArray}}; i++) + { + memcpy(addrPtr, &(ptr[i]), {{def_data.size}}); addrPtr += {{def_data.size}}; cntPub += {{def_data.size}}; } - } {% elif def_data.isArray %} + if (cntPub % 4 > 0) { - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ + for (int i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; } - cntPub += 4-(cntPub%4); + cntPub += 4 - (cntPub % 4); } arraySize = {{def_data.typeName}}.size(); - memcpy(addrPtr,&arraySize,4); + memcpy(addrPtr, &arraySize, 4); addrPtr += 4; cntPub += 4; {% if def_data.size==8 %} - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0){ + for (int i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } {% endif %} {% if def_data.cppType == "string"%} - const string* ptr = {{def_data.typeName}}.data(); + const string *ptr = {{def_data.typeName}}.data(); - for(int i=0; i0){ - for(int j=0; j<(4-(cntPub%4)) ; j++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); + for (int i = 0; i < arraySize; i++) + { + if (cntPub % 4 > 0) + { + for (int j = 0; j < (4 - (cntPub % 4)); j++) + { + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4 - (cntPub % 4); } stringSize = (ptr[i]).size(); - memcpy(addrPtr,&stringSize,4); + memcpy(addrPtr, &stringSize, 4); addrPtr += 4; cntPub += 4; - memcpy(addrPtr,(ptr[i]).c_str(),stringSize); + memcpy(addrPtr, (ptr[i]).c_str(), stringSize); addrPtr += stringSize; cntPub += stringSize; } @@ -224,87 +240,99 @@ public: {% else %} const {{def_data.cppType}}* ptr = {{def_data.typeName}}.data(); - for(int i=0; i0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ + if (cntPub % 4 >0) + { + for (int i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; } - cntPub += 4-(cntPub%4); + cntPub += 4 - (cntPub % 4); } stringSize = {{def_data.typeName}}.size(); - memcpy(addrPtr,&stringSize,4); + memcpy(addrPtr, &stringSize, 4); addrPtr += 4; cntPub += 4; - memcpy(addrPtr,{{def_data.typeName}}.c_str(),stringSize); + memcpy(addrPtr, {{def_data.typeName}}.c_str(), stringSize); addrPtr += stringSize; cntPub += stringSize; {% elif def_data.cppType == "header"%} - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ + if (cntPub % 4 > 0) + { + for (int i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; } - cntPub += 4-(cntPub%4); + cntPub += 4 - (cntPub % 4); } - memcpy(addrPtr,&sec,4); + memcpy(addrPtr, &sec, 4); addrPtr += 4; cntPub += 4; - memcpy(addrPtr,&nanosec,4); + memcpy(addrPtr, &nanosec, 4); addrPtr += 4; cntPub += 4; stringSize = frame_id.size(); - memcpy(addrPtr,&stringSize,4); + memcpy(addrPtr, &stringSize, 4); addrPtr += 4; cntPub += 4; - memcpy(addrPtr,frame_id.c_str(),stringSize); + memcpy(addrPtr, frame_id.c_str(), stringSize); addrPtr += stringSize; cntPub += stringSize; - + {% else %} {%if def_data.size==2%} - if (cntPub%4 >0 && 2 <= (4-(cntPub%4))){ - for (int i=0; i<(4-(cntPub%4))-2; i++){ - *addrPtr = 0; - addrPtr += 1; + if (cntPub % 4 >0 && 2 <= (4 - (cntPub % 4))) + { + for (int i = 0; i < (4 - (cntPub % 4)) - 2; i++) + { + *addrPtr = 0; + addrPtr += 1; } - cntPub += (4-(cntPub%4))-2; - } else if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ + cntPub += (4 - (cntPub % 4)) - 2; + } + else if (cntPub % 4 > 0) + { + for (int i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 4-(cntPub%4); + } + cntPub += 4 - (cntPub % 4); } {% elif def_data.size==4 %} - if (cntPub%4 > 0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ + if (cntPub % 4 > 0) + { + for (int i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 4-(cntPub%4); + } + cntPub += 4 - (cntPub % 4); } {% elif def_data.size==8 %} - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (int i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } {% endif %} - memcpy(addrPtr,&{{def_data.typeName}},{{def_data.size}}); + memcpy(addrPtr, &{{def_data.typeName}}, {{def_data.size}}); addrPtr += {{def_data.size}}; cntPub += {{def_data.size}}; @@ -314,7 +342,8 @@ public: return cntPub; } - uint32_t copyFromBuf(const uint8_t *addrPtr) { + uint32_t copyFromBuf(const uint8_t *addrPtr) + { uint32_t tmpSub = 0; uint32_t arraySize; uint32_t stringSize; @@ -323,23 +352,25 @@ public: {% if def_data.isCustomType%} {% if def_data.isArray %} { - if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ + if (cntSub % 4 >0) + { + for (int i = 0; i < (4 - (cntSub % 4)); i++) + { addrPtr += 1; } - cntSub += 4-(cntSub%4); + cntSub += 4 - (cntSub % 4); } - memcpy(&arraySize,addrPtr,4); - addrPtr += 4; + memcpy(&arraySize, addrPtr, 4); + addrPtr += 4; cntSub += 4; - for(int i=0;i0 and 2 <= (4-(cntSub%4))){ - for (int i=0; i<(4-(cntSub%4))-{{def_data.size}}; i++){ + if (cntSub % 4 > 0 and 2 <= (4 - (cntSub % 4))) + { + for (int i = 0; i < (4 - (cntSub % 4)) - {{def_data.size}}; i++) + { addrPtr += 1; } - cntSub += (4-(cntSub%4))-{{def_data.size}}; - }else if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - addrPtr += 1; - } - cntSub += 4-(cntSub%4); + cntSub += (4 - (cntSub % 4)) - {{def_data.size}}; + } + else if (cntSub % 4 > 0) + { + for (int i = 0; i < (4 - (cntSub % 4)); i++) + { + addrPtr += 1; + } + cntSub += 4 - (cntSub % 4); } {% elif def_data.size==4 %} - if (cntSub%4 > 0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ + if (cntSub % 4 > 0) + { + for (int i = 0; i < (4 - (cntSub % 4)); i++) + { addrPtr += 1; - } - cntSub += 4-(cntSub%4); + } + cntSub += 4 - (cntSub % 4); } {% elif def_data.size==8 %} - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (int i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } {% endif %} - for(int i=0;i<{{def_data.boundedArray}};i++){ + for (int i = 0; i < {{def_data.boundedArray}}; i++) + { {{def_data.cppType}} buf; - memcpy(&buf,addrPtr,{{def_data.size}}); + memcpy(&buf, addrPtr, {{def_data.size}}); {{def_data.typeName}}[i] = buf; addrPtr += {{def_data.size}}; cntSub += {{def_data.size}}; } - } {% elif def_data.isArray %} + if (cntSub % 4 >0) { - if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ + for (int i = 0; i < (4 - (cntSub % 4)); i++) + { addrPtr += 1; } - cntSub += 4-(cntSub%4); + cntSub += 4 - (cntSub % 4); } - memcpy(&arraySize,addrPtr,4); - addrPtr += 4; + memcpy(&arraySize, addrPtr, 4); + addrPtr += 4; cntSub += 4; {%if def_data.size==8 %} - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0){ + for (int i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } {% endif %} {{def_data.typeName}}.reserve(arraySize); - + {% if def_data.cppType == "string" %} - for(int i=0;i0){ - for(int j=0; j<(4-(cntSub%4)) ; j++){ + for (int i = 0; i < arraySize; i++) + { + if (cntSub % 4 >0) + { + for (int j=0; j < (4 - (cntSub % 4)); j++) + { addrPtr += 1; } - cntSub += 4-(cntSub%4); + cntSub += 4 - (cntSub % 4); } memcpy(&stringSize, addrPtr, 4); addrPtr += 4; cntSub += 4; string buf; buf.resize(stringSize); - memcpy(&buf[0],addrPtr,stringSize); + memcpy(&buf[0], addrPtr, stringSize); {{def_data.typeName}}.push_back(buf); addrPtr += stringSize; cntSub += stringSize; } {% else %} - for(int i=0;i0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ + if (cntSub % 4 >0) + { + for (int i = 0; i < (4 - (cntSub % 4)); i++) + { addrPtr += 1; } - cntSub += 4-(cntSub%4); + cntSub += 4 - (cntSub % 4); } memcpy(&stringSize, addrPtr, 4); addrPtr += 4; cntSub += 4; {{def_data.typeName}}.resize(stringSize); - memcpy(&{{def_data.typeName}}[0],addrPtr,stringSize); + memcpy(&{{def_data.typeName}}[0], addrPtr, stringSize); addrPtr += stringSize; cntSub += stringSize; {% elif def_data.cppType == "header"%} - if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ + if (cntSub%4 > 0) + { + for (int i = 0; i < (4 - (cntSub % 4)); i++) + { addrPtr += 1; } - cntSub += 4-(cntSub%4); + cntSub += 4 - (cntSub % 4); } - memcpy(&sec,addrPtr,4); + memcpy(&sec, addrPtr, 4); addrPtr += 4; cntSub += 4; - memcpy(&nanosec,addrPtr,4); + memcpy(&nanosec, addrPtr, 4); addrPtr += 4; cntSub += 4; @@ -469,39 +517,48 @@ public: addrPtr += 4; cntSub += 4; frame_id.resize(stringSize); - memcpy(&frame_id[0],addrPtr,stringSize); + memcpy(&frame_id[0], addrPtr, stringSize); addrPtr += stringSize; cntSub += stringSize; {% else %} {%if def_data.size==2 %} - if (cntSub%4 >0 and 2 <= (4-(cntSub%4))){ - for (int i=0; i<(4-(cntSub%4))-{{def_data.size}}; i++){ + if (cntSub%4 > 0 and 2 <= (4 - (cntSub % 4))) + { + for (int i = 0; i < (4 - (cntSub % 4)) - {{def_data.size}}; i++) + { addrPtr += 1; } - cntSub += (4-(cntSub%4))-{{def_data.size}}; - }else if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - addrPtr += 1; - } - cntSub += 4-(cntSub%4); + cntSub += (4 - (cntSub % 4)) - {{def_data.size}}; + } + else if (cntSub % 4 > 0) + { + for (int i = 0; i < (4 - (cntSub % 4)); i++) + { + addrPtr += 1; + } + cntSub += 4 - (cntSub % 4); } {% elif def_data.size==4 %} - if (cntSub%4 > 0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ + if (cntSub % 4 > 0) + { + for (int i = 0; i < (4 - (cntSub % 4)); i++) + { addrPtr += 1; - } - cntSub += 4-(cntSub%4); + } + cntSub += 4 - (cntSub % 4); } {% elif def_data.size==8 %} - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (int i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } {% endif %} - memcpy(&{{def_data.typeName}},addrPtr,{{def_data.size}}); + memcpy(&{{def_data.typeName}}, addrPtr, {{def_data.size}}); addrPtr += {{def_data.size}}; cntSub += {{def_data.size}}; {% endif %} @@ -510,22 +567,26 @@ public: return cntSub; } - void memAlign(uint8_t *addrPtr){ - if (cntPub%4 > 0){ + void memAlign(uint8_t *addrPtr) + { + if (cntPub % 4 > 0) + { addrPtr += cntPub; - for(int i=0; i<(4-(cntPub%4)) ; i++){ + for (int i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; } - cntPub += 4-(cntPub%4); + cntPub += 4 - (cntPub % 4); } return; } - uint32_t getTotalSize(){ + uint32_t getTotalSize() + { uint32_t tmpCntPub = cntPub; cntPub = 0; - return tmpCntPub ; + return tmpCntPub; } uint32_t getPubCnt() @@ -542,10 +603,8 @@ public: uint32_t calcTotalSize() { uint32_t tmp; - tmp = 4 // CDR encoding version. - + calcRawTotalSize(); - tmp += ( 0 == (tmp % 4) ? // Padding - 0 : (4 - (tmp % 4)) ); + tmp = 4 + calcRawTotalSize(); // CDR encoding version. + tmp += (0 == (tmp % 4) ? 0 : (4 - (tmp % 4))); // Padding return tmp; } @@ -558,11 +617,12 @@ public: return; } - FragCopyReturnType copyToFragBuf(uint8_t *addrPtr, uint32_t size) + 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}}"; }; From af76d1870e7a2804699fc7c0beb9317729e30757 Mon Sep 17 00:00:00 2001 From: takasehideki Date: Thu, 5 Oct 2023 20:37:24 +0900 Subject: [PATCH 2/4] eliminate [-Wsign-compare] warning message from template --- mros2_header_generator/header_template.tpl | 58 +++++++++++----------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index bd401c9..4efb2e5 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -117,7 +117,7 @@ public: {% if def_data.isArray%} if (cntPub % 4 > 0) { - for (int i = 0; i < (4 - (cntPub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) { *addrPtr = 0; addrPtr += 1; @@ -145,7 +145,7 @@ public: {%if def_data.size==2%} if (cntPub % 4 > 0 && 2 <= (4 - (cntPub % 4))) { - for (int i = 0; i < (4 - (cntPub % 4))-2; i++) + for (uint32_t i = 0; i < (4 - (cntPub % 4))-2; i++) { *addrPtr = 0; addrPtr += 1; @@ -153,7 +153,7 @@ public: cntPub += (4 - (cntPub % 4)) - 2; } else if (cntPub % 4 > 0) { - for (int i = 0; i < (4 - (cntPub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) { *addrPtr = 0; addrPtr += 1; @@ -163,7 +163,7 @@ public: {% elif def_data.size==4 %} if (cntPub % 4 > 0) { - for (int i = 0; i < (4 - (cntPub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) { *addrPtr = 0; addrPtr += 1; @@ -173,7 +173,7 @@ public: {% elif def_data.size==8 %} if (cntPub % 8 > 0) { - for (int i = 0; i < (8 - (cntPub % 8)); i++) + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) { *addrPtr = 0; addrPtr += 1; @@ -191,7 +191,7 @@ public: {% elif def_data.isArray %} if (cntPub % 4 > 0) { - for (int i = 0; i < (4 - (cntPub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) { *addrPtr = 0; addrPtr += 1; @@ -205,7 +205,7 @@ public: {% if def_data.size==8 %} if (cntPub % 8 > 0){ - for (int i = 0; i < (8 - (cntPub % 8)); i++) + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) { *addrPtr = 0; addrPtr += 1; @@ -221,7 +221,7 @@ public: { if (cntPub % 4 > 0) { - for (int j = 0; j < (4 - (cntPub % 4)); j++) + for (uint32_t j = 0; j < (4 - (cntPub % 4)); j++) { *addrPtr = 0; addrPtr += 1; @@ -249,7 +249,7 @@ public: {% elif def_data.cppType == "string"%} if (cntPub % 4 >0) { - for (int i = 0; i < (4 - (cntPub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) { *addrPtr = 0; addrPtr += 1; @@ -267,7 +267,7 @@ public: {% elif def_data.cppType == "header"%} if (cntPub % 4 > 0) { - for (int i = 0; i < (4 - (cntPub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) { *addrPtr = 0; addrPtr += 1; @@ -295,7 +295,7 @@ public: {%if def_data.size==2%} if (cntPub % 4 >0 && 2 <= (4 - (cntPub % 4))) { - for (int i = 0; i < (4 - (cntPub % 4)) - 2; i++) + for (uint32_t i = 0; i < (4 - (cntPub % 4)) - 2; i++) { *addrPtr = 0; addrPtr += 1; @@ -304,7 +304,7 @@ public: } else if (cntPub % 4 > 0) { - for (int i = 0; i < (4 - (cntPub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) { *addrPtr = 0; addrPtr += 1; @@ -314,7 +314,7 @@ public: {% elif def_data.size==4 %} if (cntPub % 4 > 0) { - for (int i = 0; i < (4 - (cntPub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) { *addrPtr = 0; addrPtr += 1; @@ -324,7 +324,7 @@ public: {% elif def_data.size==8 %} if (cntPub % 8 > 0) { - for (int i = 0; i < (8 - (cntPub % 8)); i++) + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) { *addrPtr = 0; addrPtr += 1; @@ -354,7 +354,7 @@ public: { if (cntSub % 4 >0) { - for (int i = 0; i < (4 - (cntSub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntSub % 4)); i++) { addrPtr += 1; } @@ -381,7 +381,7 @@ public: {%if def_data.size==2 %} if (cntSub % 4 > 0 and 2 <= (4 - (cntSub % 4))) { - for (int i = 0; i < (4 - (cntSub % 4)) - {{def_data.size}}; i++) + for (uint32_t i = 0; i < (4 - (cntSub % 4)) - {{def_data.size}}; i++) { addrPtr += 1; } @@ -389,7 +389,7 @@ public: } else if (cntSub % 4 > 0) { - for (int i = 0; i < (4 - (cntSub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntSub % 4)); i++) { addrPtr += 1; } @@ -398,7 +398,7 @@ public: {% elif def_data.size==4 %} if (cntSub % 4 > 0) { - for (int i = 0; i < (4 - (cntSub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntSub % 4)); i++) { addrPtr += 1; } @@ -407,7 +407,7 @@ public: {% elif def_data.size==8 %} if (cntSub % 8 > 0) { - for (int i = 0; i < (8 - (cntSub % 8)); i++) + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) { addrPtr += 1; } @@ -425,7 +425,7 @@ public: {% elif def_data.isArray %} if (cntSub % 4 >0) { - for (int i = 0; i < (4 - (cntSub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntSub % 4)); i++) { addrPtr += 1; } @@ -437,7 +437,7 @@ public: {%if def_data.size==8 %} if (cntSub % 8 > 0){ - for (int i = 0; i < (8 - (cntSub % 8)); i++) + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) { addrPtr += 1; } @@ -452,7 +452,7 @@ public: { if (cntSub % 4 >0) { - for (int j=0; j < (4 - (cntSub % 4)); j++) + for (uint32_t j=0; j < (4 - (cntSub % 4)); j++) { addrPtr += 1; } @@ -482,7 +482,7 @@ public: {% elif def_data.cppType == "string"%} if (cntSub % 4 >0) { - for (int i = 0; i < (4 - (cntSub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntSub % 4)); i++) { addrPtr += 1; } @@ -499,7 +499,7 @@ public: {% elif def_data.cppType == "header"%} if (cntSub%4 > 0) { - for (int i = 0; i < (4 - (cntSub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntSub % 4)); i++) { addrPtr += 1; } @@ -525,7 +525,7 @@ public: {%if def_data.size==2 %} if (cntSub%4 > 0 and 2 <= (4 - (cntSub % 4))) { - for (int i = 0; i < (4 - (cntSub % 4)) - {{def_data.size}}; i++) + for (uint32_t i = 0; i < (4 - (cntSub % 4)) - {{def_data.size}}; i++) { addrPtr += 1; } @@ -533,7 +533,7 @@ public: } else if (cntSub % 4 > 0) { - for (int i = 0; i < (4 - (cntSub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntSub % 4)); i++) { addrPtr += 1; } @@ -542,7 +542,7 @@ public: {% elif def_data.size==4 %} if (cntSub % 4 > 0) { - for (int i = 0; i < (4 - (cntSub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntSub % 4)); i++) { addrPtr += 1; } @@ -551,7 +551,7 @@ public: {% elif def_data.size==8 %} if (cntSub % 8 > 0) { - for (int i = 0; i < (8 - (cntSub % 8)); i++) + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) { addrPtr += 1; } @@ -572,7 +572,7 @@ public: if (cntPub % 4 > 0) { addrPtr += cntPub; - for (int i = 0; i < (4 - (cntPub % 4)); i++) + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) { *addrPtr = 0; addrPtr += 1; From 862f805491831de25a8c1fd201958e776ee3c3fa Mon Sep 17 00:00:00 2001 From: takasehideki Date: Thu, 5 Oct 2023 20:38:04 +0900 Subject: [PATCH 3/4] add to use memcpy properly in C++ --- mros2_header_generator/header_template.tpl | 1 + 1 file changed, 1 insertion(+) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 4efb2e5..94df848 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -3,6 +3,7 @@ #include #include +#include {%- set ns = namespace(break=0) -%} {%- for def_data in msg.def %} {%- if def_data.isArray %} From e64805014346dd659c396b0c58a40b69070159b2 Mon Sep 17 00:00:00 2001 From: takasehideki Date: Thu, 5 Oct 2023 20:48:34 +0900 Subject: [PATCH 4/4] regenerate with improved header_generator --- mros2_msgs/geometry_msgs/msg/point.hpp | 140 ++++++++++------- mros2_msgs/geometry_msgs/msg/pose.hpp | 66 ++++---- mros2_msgs/geometry_msgs/msg/quaternion.hpp | 166 +++++++++++--------- mros2_msgs/geometry_msgs/msg/twist.hpp | 66 ++++---- mros2_msgs/geometry_msgs/msg/vector3.hpp | 140 ++++++++++------- 5 files changed, 329 insertions(+), 249 deletions(-) diff --git a/mros2_msgs/geometry_msgs/msg/point.hpp b/mros2_msgs/geometry_msgs/msg/point.hpp index a3fb695..e194a1b 100644 --- a/mros2_msgs/geometry_msgs/msg/point.hpp +++ b/mros2_msgs/geometry_msgs/msg/point.hpp @@ -3,6 +3,7 @@ #include #include +#include using namespace std; @@ -20,19 +21,20 @@ class Point typedef std::pair FragCopyReturnType; template - uint32_t copyPrimToFragBufLocal(uint8_t*& addrPtr, + uint32_t copyPrimToFragBufLocal(uint8_t *&addrPtr, const uint32_t cntPub, const uint32_t size, - const T& data) + 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) ) { + 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++){ + for (int i = 0; i < lenPad; i++) + { *addrPtr = 0; addrPtr += 1; } @@ -43,17 +45,19 @@ class Point return sizeof(T) + lenPad; } - template - FragCopyReturnType copyArrayToFragBufLocal(uint8_t*& addrPtr, + template + FragCopyReturnType copyArrayToFragBufLocal(uint8_t *&addrPtr, const uint32_t size, - T& data, - uint32_t& cntPubMemberLocal) + T &data, + uint32_t &cntPubMemberLocal) { uint32_t pubDataSize = data.size(); uint32_t cntLocalFrag = 0; - if (cntPubMemberLocal < sizeof(uint32_t)) { - if (size < sizeof(uint32_t)) { + if (cntPubMemberLocal < sizeof(uint32_t)) + { + if (size < sizeof(uint32_t)) + { return {false, 0}; } memcpy(addrPtr, &pubDataSize, sizeof(uint32_t)); @@ -62,10 +66,10 @@ class Point cntLocalFrag += sizeof(uint32_t); } - uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); - // cntPubMemberLocal > 4 here + uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); // cntPubMemberLocal > 4 here uint32_t tmp = std::min(pubDataSize - cntFrag, size - cntLocalFrag); - if (0 < tmp) { + if (0 < tmp) + { memcpy(addrPtr, data.data() + cntFrag, tmp); addrPtr += tmp; cntPubMemberLocal += tmp; @@ -75,11 +79,11 @@ class Point return {(cntPubMemberLocal - sizeof(uint32_t)) >= pubDataSize, cntLocalFrag}; } - + double x; - + double y; - + double z; @@ -91,15 +95,17 @@ class Point - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } - memcpy(addrPtr,&x,8); + memcpy(addrPtr, &x, 8); addrPtr += 8; cntPub += 8; @@ -107,15 +113,17 @@ class Point - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } - memcpy(addrPtr,&y,8); + memcpy(addrPtr, &y, 8); addrPtr += 8; cntPub += 8; @@ -123,15 +131,17 @@ class Point - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } - memcpy(addrPtr,&z,8); + memcpy(addrPtr, &z, 8); addrPtr += 8; cntPub += 8; @@ -141,7 +151,8 @@ class Point return cntPub; } - uint32_t copyFromBuf(const uint8_t *addrPtr) { + uint32_t copyFromBuf(const uint8_t *addrPtr) + { uint32_t tmpSub = 0; uint32_t arraySize; uint32_t stringSize; @@ -149,42 +160,48 @@ class Point - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } - memcpy(&x,addrPtr,8); + memcpy(&x, addrPtr, 8); addrPtr += 8; cntSub += 8; - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } - memcpy(&y,addrPtr,8); + memcpy(&y, addrPtr, 8); addrPtr += 8; cntSub += 8; - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } - memcpy(&z,addrPtr,8); + memcpy(&z, addrPtr, 8); addrPtr += 8; cntSub += 8; @@ -193,22 +210,26 @@ class Point return cntSub; } - void memAlign(uint8_t *addrPtr){ - if (cntPub%4 > 0){ + void memAlign(uint8_t *addrPtr) + { + if (cntPub % 4 > 0) + { addrPtr += cntPub; - for(int i=0; i<(4-(cntPub%4)) ; i++){ + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; } - cntPub += 4-(cntPub%4); + cntPub += 4 - (cntPub % 4); } return; } - uint32_t getTotalSize(){ + uint32_t getTotalSize() + { uint32_t tmpCntPub = cntPub; cntPub = 0; - return tmpCntPub ; + return tmpCntPub; } uint32_t getPubCnt() @@ -225,10 +246,8 @@ class Point uint32_t calcTotalSize() { uint32_t tmp; - tmp = 4 // CDR encoding version. - + calcRawTotalSize(); - tmp += ( 0 == (tmp % 4) ? // Padding - 0 : (4 - (tmp % 4)) ); + tmp = 4 + calcRawTotalSize(); // CDR encoding version. + tmp += (0 == (tmp % 4) ? 0 : (4 - (tmp % 4))); // Padding return tmp; } @@ -241,11 +260,12 @@ class Point return; } - FragCopyReturnType copyToFragBuf(uint8_t *addrPtr, uint32_t size) + FragCopyReturnType copyToFragBuf(uint8_t *addrPtr, uint32_t size) { // TODO: store template code here return {false, 0}; } + private: std::string type_name = "geometry_msgs::msg::dds_::Point"; }; diff --git a/mros2_msgs/geometry_msgs/msg/pose.hpp b/mros2_msgs/geometry_msgs/msg/pose.hpp index 35530b1..7be3ae8 100644 --- a/mros2_msgs/geometry_msgs/msg/pose.hpp +++ b/mros2_msgs/geometry_msgs/msg/pose.hpp @@ -3,6 +3,7 @@ #include #include +#include #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -22,19 +23,20 @@ class Pose typedef std::pair FragCopyReturnType; template - uint32_t copyPrimToFragBufLocal(uint8_t*& addrPtr, + uint32_t copyPrimToFragBufLocal(uint8_t *&addrPtr, const uint32_t cntPub, const uint32_t size, - const T& data) + 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) ) { + 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++){ + for (int i = 0; i < lenPad; i++) + { *addrPtr = 0; addrPtr += 1; } @@ -45,17 +47,19 @@ class Pose return sizeof(T) + lenPad; } - template - FragCopyReturnType copyArrayToFragBufLocal(uint8_t*& addrPtr, + template + FragCopyReturnType copyArrayToFragBufLocal(uint8_t *&addrPtr, const uint32_t size, - T& data, - uint32_t& cntPubMemberLocal) + T &data, + uint32_t &cntPubMemberLocal) { uint32_t pubDataSize = data.size(); uint32_t cntLocalFrag = 0; - if (cntPubMemberLocal < sizeof(uint32_t)) { - if (size < sizeof(uint32_t)) { + if (cntPubMemberLocal < sizeof(uint32_t)) + { + if (size < sizeof(uint32_t)) + { return {false, 0}; } memcpy(addrPtr, &pubDataSize, sizeof(uint32_t)); @@ -64,10 +68,10 @@ class Pose cntLocalFrag += sizeof(uint32_t); } - uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); - // cntPubMemberLocal > 4 here + uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); // cntPubMemberLocal > 4 here uint32_t tmp = std::min(pubDataSize - cntFrag, size - cntLocalFrag); - if (0 < tmp) { + if (0 < tmp) + { memcpy(addrPtr, data.data() + cntFrag, tmp); addrPtr += tmp; cntPubMemberLocal += tmp; @@ -77,9 +81,9 @@ class Pose return {(cntPubMemberLocal - sizeof(uint32_t)) >= pubDataSize, cntLocalFrag}; } - + geometry_msgs::msg::Point position; - + geometry_msgs::msg::Quaternion orientation; @@ -109,7 +113,8 @@ class Pose return cntPub; } - uint32_t copyFromBuf(const uint8_t *addrPtr) { + uint32_t copyFromBuf(const uint8_t *addrPtr) + { uint32_t tmpSub = 0; uint32_t arraySize; uint32_t stringSize; @@ -137,22 +142,26 @@ class Pose return cntSub; } - void memAlign(uint8_t *addrPtr){ - if (cntPub%4 > 0){ + void memAlign(uint8_t *addrPtr) + { + if (cntPub % 4 > 0) + { addrPtr += cntPub; - for(int i=0; i<(4-(cntPub%4)) ; i++){ + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; } - cntPub += 4-(cntPub%4); + cntPub += 4 - (cntPub % 4); } return; } - uint32_t getTotalSize(){ + uint32_t getTotalSize() + { uint32_t tmpCntPub = cntPub; cntPub = 0; - return tmpCntPub ; + return tmpCntPub; } uint32_t getPubCnt() @@ -169,10 +178,8 @@ class Pose uint32_t calcTotalSize() { uint32_t tmp; - tmp = 4 // CDR encoding version. - + calcRawTotalSize(); - tmp += ( 0 == (tmp % 4) ? // Padding - 0 : (4 - (tmp % 4)) ); + tmp = 4 + calcRawTotalSize(); // CDR encoding version. + tmp += (0 == (tmp % 4) ? 0 : (4 - (tmp % 4))); // Padding return tmp; } @@ -185,11 +192,12 @@ class Pose return; } - FragCopyReturnType copyToFragBuf(uint8_t *addrPtr, uint32_t size) + FragCopyReturnType copyToFragBuf(uint8_t *addrPtr, uint32_t size) { // TODO: store template code here return {false, 0}; } + private: std::string type_name = "geometry_msgs::msg::dds_::Pose"; }; diff --git a/mros2_msgs/geometry_msgs/msg/quaternion.hpp b/mros2_msgs/geometry_msgs/msg/quaternion.hpp index afea149..ed9197f 100644 --- a/mros2_msgs/geometry_msgs/msg/quaternion.hpp +++ b/mros2_msgs/geometry_msgs/msg/quaternion.hpp @@ -3,6 +3,7 @@ #include #include +#include using namespace std; @@ -20,19 +21,20 @@ class Quaternion typedef std::pair FragCopyReturnType; template - uint32_t copyPrimToFragBufLocal(uint8_t*& addrPtr, + uint32_t copyPrimToFragBufLocal(uint8_t *&addrPtr, const uint32_t cntPub, const uint32_t size, - const T& data) + 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) ) { + 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++){ + for (int i = 0; i < lenPad; i++) + { *addrPtr = 0; addrPtr += 1; } @@ -43,17 +45,19 @@ class Quaternion return sizeof(T) + lenPad; } - template - FragCopyReturnType copyArrayToFragBufLocal(uint8_t*& addrPtr, + template + FragCopyReturnType copyArrayToFragBufLocal(uint8_t *&addrPtr, const uint32_t size, - T& data, - uint32_t& cntPubMemberLocal) + T &data, + uint32_t &cntPubMemberLocal) { uint32_t pubDataSize = data.size(); uint32_t cntLocalFrag = 0; - if (cntPubMemberLocal < sizeof(uint32_t)) { - if (size < sizeof(uint32_t)) { + if (cntPubMemberLocal < sizeof(uint32_t)) + { + if (size < sizeof(uint32_t)) + { return {false, 0}; } memcpy(addrPtr, &pubDataSize, sizeof(uint32_t)); @@ -62,10 +66,10 @@ class Quaternion cntLocalFrag += sizeof(uint32_t); } - uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); - // cntPubMemberLocal > 4 here + uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); // cntPubMemberLocal > 4 here uint32_t tmp = std::min(pubDataSize - cntFrag, size - cntLocalFrag); - if (0 < tmp) { + if (0 < tmp) + { memcpy(addrPtr, data.data() + cntFrag, tmp); addrPtr += tmp; cntPubMemberLocal += tmp; @@ -75,13 +79,13 @@ class Quaternion return {(cntPubMemberLocal - sizeof(uint32_t)) >= pubDataSize, cntLocalFrag}; } - + double x; - + double y; - + double z; - + double w; @@ -93,15 +97,17 @@ class Quaternion - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } - memcpy(addrPtr,&x,8); + memcpy(addrPtr, &x, 8); addrPtr += 8; cntPub += 8; @@ -109,15 +115,17 @@ class Quaternion - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } - memcpy(addrPtr,&y,8); + memcpy(addrPtr, &y, 8); addrPtr += 8; cntPub += 8; @@ -125,15 +133,17 @@ class Quaternion - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } - memcpy(addrPtr,&z,8); + memcpy(addrPtr, &z, 8); addrPtr += 8; cntPub += 8; @@ -141,15 +151,17 @@ class Quaternion - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } - memcpy(addrPtr,&w,8); + memcpy(addrPtr, &w, 8); addrPtr += 8; cntPub += 8; @@ -159,7 +171,8 @@ class Quaternion return cntPub; } - uint32_t copyFromBuf(const uint8_t *addrPtr) { + uint32_t copyFromBuf(const uint8_t *addrPtr) + { uint32_t tmpSub = 0; uint32_t arraySize; uint32_t stringSize; @@ -167,56 +180,64 @@ class Quaternion - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } - memcpy(&x,addrPtr,8); + memcpy(&x, addrPtr, 8); addrPtr += 8; cntSub += 8; - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } - memcpy(&y,addrPtr,8); + memcpy(&y, addrPtr, 8); addrPtr += 8; cntSub += 8; - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } - memcpy(&z,addrPtr,8); + memcpy(&z, addrPtr, 8); addrPtr += 8; cntSub += 8; - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } - memcpy(&w,addrPtr,8); + memcpy(&w, addrPtr, 8); addrPtr += 8; cntSub += 8; @@ -225,22 +246,26 @@ class Quaternion return cntSub; } - void memAlign(uint8_t *addrPtr){ - if (cntPub%4 > 0){ + void memAlign(uint8_t *addrPtr) + { + if (cntPub % 4 > 0) + { addrPtr += cntPub; - for(int i=0; i<(4-(cntPub%4)) ; i++){ + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; } - cntPub += 4-(cntPub%4); + cntPub += 4 - (cntPub % 4); } return; } - uint32_t getTotalSize(){ + uint32_t getTotalSize() + { uint32_t tmpCntPub = cntPub; cntPub = 0; - return tmpCntPub ; + return tmpCntPub; } uint32_t getPubCnt() @@ -257,10 +282,8 @@ class Quaternion uint32_t calcTotalSize() { uint32_t tmp; - tmp = 4 // CDR encoding version. - + calcRawTotalSize(); - tmp += ( 0 == (tmp % 4) ? // Padding - 0 : (4 - (tmp % 4)) ); + tmp = 4 + calcRawTotalSize(); // CDR encoding version. + tmp += (0 == (tmp % 4) ? 0 : (4 - (tmp % 4))); // Padding return tmp; } @@ -273,11 +296,12 @@ class Quaternion return; } - FragCopyReturnType copyToFragBuf(uint8_t *addrPtr, uint32_t size) + FragCopyReturnType copyToFragBuf(uint8_t *addrPtr, uint32_t size) { // TODO: store template code here return {false, 0}; } + private: std::string type_name = "geometry_msgs::msg::dds_::Quaternion"; }; diff --git a/mros2_msgs/geometry_msgs/msg/twist.hpp b/mros2_msgs/geometry_msgs/msg/twist.hpp index 478cbf4..31fba42 100644 --- a/mros2_msgs/geometry_msgs/msg/twist.hpp +++ b/mros2_msgs/geometry_msgs/msg/twist.hpp @@ -3,6 +3,7 @@ #include #include +#include #include "geometry_msgs/msg/vector3.hpp" #include "geometry_msgs/msg/vector3.hpp" @@ -22,19 +23,20 @@ class Twist typedef std::pair FragCopyReturnType; template - uint32_t copyPrimToFragBufLocal(uint8_t*& addrPtr, + uint32_t copyPrimToFragBufLocal(uint8_t *&addrPtr, const uint32_t cntPub, const uint32_t size, - const T& data) + 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) ) { + 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++){ + for (int i = 0; i < lenPad; i++) + { *addrPtr = 0; addrPtr += 1; } @@ -45,17 +47,19 @@ class Twist return sizeof(T) + lenPad; } - template - FragCopyReturnType copyArrayToFragBufLocal(uint8_t*& addrPtr, + template + FragCopyReturnType copyArrayToFragBufLocal(uint8_t *&addrPtr, const uint32_t size, - T& data, - uint32_t& cntPubMemberLocal) + T &data, + uint32_t &cntPubMemberLocal) { uint32_t pubDataSize = data.size(); uint32_t cntLocalFrag = 0; - if (cntPubMemberLocal < sizeof(uint32_t)) { - if (size < sizeof(uint32_t)) { + if (cntPubMemberLocal < sizeof(uint32_t)) + { + if (size < sizeof(uint32_t)) + { return {false, 0}; } memcpy(addrPtr, &pubDataSize, sizeof(uint32_t)); @@ -64,10 +68,10 @@ class Twist cntLocalFrag += sizeof(uint32_t); } - uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); - // cntPubMemberLocal > 4 here + uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); // cntPubMemberLocal > 4 here uint32_t tmp = std::min(pubDataSize - cntFrag, size - cntLocalFrag); - if (0 < tmp) { + if (0 < tmp) + { memcpy(addrPtr, data.data() + cntFrag, tmp); addrPtr += tmp; cntPubMemberLocal += tmp; @@ -77,9 +81,9 @@ class Twist return {(cntPubMemberLocal - sizeof(uint32_t)) >= pubDataSize, cntLocalFrag}; } - + geometry_msgs::msg::Vector3 linear; - + geometry_msgs::msg::Vector3 angular; @@ -109,7 +113,8 @@ class Twist return cntPub; } - uint32_t copyFromBuf(const uint8_t *addrPtr) { + uint32_t copyFromBuf(const uint8_t *addrPtr) + { uint32_t tmpSub = 0; uint32_t arraySize; uint32_t stringSize; @@ -137,22 +142,26 @@ class Twist return cntSub; } - void memAlign(uint8_t *addrPtr){ - if (cntPub%4 > 0){ + void memAlign(uint8_t *addrPtr) + { + if (cntPub % 4 > 0) + { addrPtr += cntPub; - for(int i=0; i<(4-(cntPub%4)) ; i++){ + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; } - cntPub += 4-(cntPub%4); + cntPub += 4 - (cntPub % 4); } return; } - uint32_t getTotalSize(){ + uint32_t getTotalSize() + { uint32_t tmpCntPub = cntPub; cntPub = 0; - return tmpCntPub ; + return tmpCntPub; } uint32_t getPubCnt() @@ -169,10 +178,8 @@ class Twist uint32_t calcTotalSize() { uint32_t tmp; - tmp = 4 // CDR encoding version. - + calcRawTotalSize(); - tmp += ( 0 == (tmp % 4) ? // Padding - 0 : (4 - (tmp % 4)) ); + tmp = 4 + calcRawTotalSize(); // CDR encoding version. + tmp += (0 == (tmp % 4) ? 0 : (4 - (tmp % 4))); // Padding return tmp; } @@ -185,11 +192,12 @@ class Twist return; } - FragCopyReturnType copyToFragBuf(uint8_t *addrPtr, uint32_t size) + FragCopyReturnType copyToFragBuf(uint8_t *addrPtr, uint32_t size) { // TODO: store template code here return {false, 0}; } + private: std::string type_name = "geometry_msgs::msg::dds_::Twist"; }; diff --git a/mros2_msgs/geometry_msgs/msg/vector3.hpp b/mros2_msgs/geometry_msgs/msg/vector3.hpp index 291ebee..fdb2a21 100644 --- a/mros2_msgs/geometry_msgs/msg/vector3.hpp +++ b/mros2_msgs/geometry_msgs/msg/vector3.hpp @@ -3,6 +3,7 @@ #include #include +#include using namespace std; @@ -20,19 +21,20 @@ class Vector3 typedef std::pair FragCopyReturnType; template - uint32_t copyPrimToFragBufLocal(uint8_t*& addrPtr, + uint32_t copyPrimToFragBufLocal(uint8_t *&addrPtr, const uint32_t cntPub, const uint32_t size, - const T& data) + 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) ) { + 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++){ + for (int i = 0; i < lenPad; i++) + { *addrPtr = 0; addrPtr += 1; } @@ -43,17 +45,19 @@ class Vector3 return sizeof(T) + lenPad; } - template - FragCopyReturnType copyArrayToFragBufLocal(uint8_t*& addrPtr, + template + FragCopyReturnType copyArrayToFragBufLocal(uint8_t *&addrPtr, const uint32_t size, - T& data, - uint32_t& cntPubMemberLocal) + T &data, + uint32_t &cntPubMemberLocal) { uint32_t pubDataSize = data.size(); uint32_t cntLocalFrag = 0; - if (cntPubMemberLocal < sizeof(uint32_t)) { - if (size < sizeof(uint32_t)) { + if (cntPubMemberLocal < sizeof(uint32_t)) + { + if (size < sizeof(uint32_t)) + { return {false, 0}; } memcpy(addrPtr, &pubDataSize, sizeof(uint32_t)); @@ -62,10 +66,10 @@ class Vector3 cntLocalFrag += sizeof(uint32_t); } - uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); - // cntPubMemberLocal > 4 here + uint32_t cntFrag = (cntPubMemberLocal - sizeof(uint32_t)); // cntPubMemberLocal > 4 here uint32_t tmp = std::min(pubDataSize - cntFrag, size - cntLocalFrag); - if (0 < tmp) { + if (0 < tmp) + { memcpy(addrPtr, data.data() + cntFrag, tmp); addrPtr += tmp; cntPubMemberLocal += tmp; @@ -75,11 +79,11 @@ class Vector3 return {(cntPubMemberLocal - sizeof(uint32_t)) >= pubDataSize, cntLocalFrag}; } - + double x; - + double y; - + double z; @@ -91,15 +95,17 @@ class Vector3 - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } - memcpy(addrPtr,&x,8); + memcpy(addrPtr, &x, 8); addrPtr += 8; cntPub += 8; @@ -107,15 +113,17 @@ class Vector3 - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } - memcpy(addrPtr,&y,8); + memcpy(addrPtr, &y, 8); addrPtr += 8; cntPub += 8; @@ -123,15 +131,17 @@ class Vector3 - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ + if (cntPub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntPub % 8)); i++) + { *addrPtr = 0; addrPtr += 1; - } - cntPub += 8-(cntPub%8); + } + cntPub += 8 - (cntPub % 8); } - memcpy(addrPtr,&z,8); + memcpy(addrPtr, &z, 8); addrPtr += 8; cntPub += 8; @@ -141,7 +151,8 @@ class Vector3 return cntPub; } - uint32_t copyFromBuf(const uint8_t *addrPtr) { + uint32_t copyFromBuf(const uint8_t *addrPtr) + { uint32_t tmpSub = 0; uint32_t arraySize; uint32_t stringSize; @@ -149,42 +160,48 @@ class Vector3 - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } - memcpy(&x,addrPtr,8); + memcpy(&x, addrPtr, 8); addrPtr += 8; cntSub += 8; - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } - memcpy(&y,addrPtr,8); + memcpy(&y, addrPtr, 8); addrPtr += 8; cntSub += 8; - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + if (cntSub % 8 > 0) + { + for (uint32_t i = 0; i < (8 - (cntSub % 8)); i++) + { addrPtr += 1; - } - cntSub += 8-(cntSub%8); + } + cntSub += 8 - (cntSub % 8); } - memcpy(&z,addrPtr,8); + memcpy(&z, addrPtr, 8); addrPtr += 8; cntSub += 8; @@ -193,22 +210,26 @@ class Vector3 return cntSub; } - void memAlign(uint8_t *addrPtr){ - if (cntPub%4 > 0){ + void memAlign(uint8_t *addrPtr) + { + if (cntPub % 4 > 0) + { addrPtr += cntPub; - for(int i=0; i<(4-(cntPub%4)) ; i++){ + for (uint32_t i = 0; i < (4 - (cntPub % 4)); i++) + { *addrPtr = 0; addrPtr += 1; } - cntPub += 4-(cntPub%4); + cntPub += 4 - (cntPub % 4); } return; } - uint32_t getTotalSize(){ + uint32_t getTotalSize() + { uint32_t tmpCntPub = cntPub; cntPub = 0; - return tmpCntPub ; + return tmpCntPub; } uint32_t getPubCnt() @@ -225,10 +246,8 @@ class Vector3 uint32_t calcTotalSize() { uint32_t tmp; - tmp = 4 // CDR encoding version. - + calcRawTotalSize(); - tmp += ( 0 == (tmp % 4) ? // Padding - 0 : (4 - (tmp % 4)) ); + tmp = 4 + calcRawTotalSize(); // CDR encoding version. + tmp += (0 == (tmp % 4) ? 0 : (4 - (tmp % 4))); // Padding return tmp; } @@ -241,11 +260,12 @@ class Vector3 return; } - FragCopyReturnType copyToFragBuf(uint8_t *addrPtr, uint32_t size) + FragCopyReturnType copyToFragBuf(uint8_t *addrPtr, uint32_t size) { // TODO: store template code here return {false, 0}; } + private: std::string type_name = "geometry_msgs::msg::dds_::Vector3"; };