Skip to content

Commit

Permalink
Merge pull request #33 from smoriemb/feature/mros2-frag-msg-proto
Browse files Browse the repository at this point in the history 
Feature/mros2 frag msg proto
  • Loading branch information
@takasehideki
takasehideki committed Apr 11, 2023
2 parents 28a1e3e + 6067701 commit 7647397
Show file tree
Hide file tree
Showing 12 changed files with 1,158 additions and 1 deletion.
2 changes: 1 addition & 1 deletion mros2.lib
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
https://github.com/mROS-base/mros2#v0.4.0
https://github.com/smoriemb/mros2#feature/mros2-frag-msg-proto
94 changes: 94 additions & 0 deletions workspace/custom_msgs/geometry_msgs/msg/point.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,65 @@ class Point
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};
}


double x
Expand Down Expand Up @@ -158,6 +217,41 @@ class Point
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 = "geometry_msgs::msg::dds_::Point";
};
Expand Down
94 changes: 94 additions & 0 deletions workspace/custom_msgs/geometry_msgs/msg/pose.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,65 @@ class Pose
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};
}


geometry_msgs::msg::Point position
Expand Down Expand Up @@ -99,6 +158,41 @@ class Pose
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 = "geometry_msgs::msg::dds_::Pose";
};
Expand Down
94 changes: 94 additions & 0 deletions workspace/custom_msgs/geometry_msgs/msg/quaternion.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,65 @@ class Quaternion
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};
}


double x
Expand Down Expand Up @@ -193,6 +252,41 @@ class Quaternion
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 = "geometry_msgs::msg::dds_::Quaternion";
};
Expand Down
Loading

0 comments on commit 7647397

Please sign in to comment.