diff --git a/README.md b/README.md index 59079b3..ac982ea 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ Here are the functionalities that mROS 2 offers for you, and current limitations - `wstring` (UTF-16) is not provided due to its difficulty in verification, but it is unlikely to be used. - `array` types are not supported - Some custom message types (e.g., Twist, Pose) - - Please check [mros2-mbed#generating-header-files-for-custom-msgtypes](https://github.com/mROS-base/mros2-mbed#generating-header-files-for-custom-msgtypes) for more details. + - Please check [#generating-header-files-for-custom-msgtypes](https://github.com/mROS-base/mros2#generating-header-files-for-custom-msgtypes) for more details. - Fragmented message types (that exceed one packet) are experimentally supported. See [PR#36](https://github.com/mROS-base/mros2/pull/36) for more details. - We think variable-length types cannot be handled, probably due to the limitation of lwIP. - Service, Actions, and Parameters are not supported @@ -45,6 +45,88 @@ Please see each repository to learn how to use it. Please let us know if you have a request for support for other boards/kernels, or if you could implement this layer on other platforms. +## Generating header files for custom MsgTypes + +You can use almost any [built-in-types in ROS 2](https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html#field-types) on the embedded device. +Not that `array` types are not supported yet. + +You can also use following message types that are commonly used. We have prepared and located them in `mros2_msgs/`. + +- [geometry_msgs/msg/Twist](https://docs.ros2.org/latest/api/geometry_msgs/msg/Twist.html) +- [geometry_msgs/msg/Vector3](https://docs.ros2.org/latest/api/geometry_msgs/msg/Vector3.html) +- [geometry_msgs/msg/Pose](https://docs.ros2.org/latest/api/geometry_msgs/msg/Pose.html) +- [geometry_msgs/msg/Point](https://docs.ros2.org/latest/api/geometry_msgs/msg/Point.html) +- [geometry_msgs/msg/Quaternion](https://docs.ros2.org/latest/api/geometry_msgs/msg/Quaternion.html) +- [sensor_msgs/msg/Image](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) (experimental) + +In additon, you can define a customized message type in the same way as in ROS 2, and use its header file for your application. +The rest of this section describes how to generate header files for your own MsgTypes. +The example assumes the location as `/mros2_msgs/` and target as `geometry_msgs::msg::Twist`. +The location is arbitrary, but be careful with the paths of the Python script and the .msg file. + +### Prepare .msg files + +`.msg` files are simple text files that describe the fields of a ROS message (see [About ROS 2 interface](https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html)). In mros2, they are used to generate header files for messages in embedded applications. + +Prepare `Twist.msg` file and make sure it is in `/mros2_msgs/geometry_msgs/msg/`. + +``` +$ pwd +/mros2_msgs + +$ cat geometry_msgs/msg/Twist.msg +geometry_msgs/msg/Vector3 linear +geometry_msgs/msg/Vector3 angular +``` + +In this example, `Twist` has a nested structure with `Vector3` as a child element. So you also need to prepare its file. + +``` +$ cat geometry_msgs/msg/Vector3.msg +float64 x +float64 y +float64 z +``` + +### Generate header files + +To generate header files for `Twist` and `Vector3`, run the following command in `/mros2_msgs/` (again, be careful about the paths!). + +``` +$ cd /mros2_msgs +$ python3 ../mros2_header_generator/header_generator.py geometry_msgs/msg/Twist.msg +``` + +Make sure header files for custom MsgType are generated in `geometry_msgs/`. + +``` +$ ls -R geometry_msgs/ +geometry_msgs: +msg + +geometry_msgs/msg: +twist.hpp vector3.hpp Twist.msg Vector3.msg +``` + +You can now use them in your applicaton like this. + +``` +#include "mros2.hpp" +#include "mros2-platform.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "geometry_msgs/msg/twist.hpp" + +int main(int argc, char * argv[]) +{ + + pub = node.create_publisher("cmd_vel", 10); + +``` + +When you generated your own header files at an arbitrary location, you need to add its path as an include path for build (e.g., CMakeLists.txt). + +If you generated new header files at `/mros2_msgs/`, we are very welcome to your PR!! + ## License The source code of this repository itself is published under [Apache License 2.0](https://github.com/mROS-base/mros2/blob/main/LICENSE). diff --git a/mros2_header_generator/header_generator.py b/mros2_header_generator/header_generator.py index a7e42f5..8cecaad 100644 --- a/mros2_header_generator/header_generator.py +++ b/mros2_header_generator/header_generator.py @@ -26,10 +26,8 @@ def main(): template = env.get_template('header_template.tpl') datatext = template.render({ "msg": msg }) - msgPkgPath = "custom_msgs" + "/" + msg['pkg'] + msgPkgPath = msg['pkg'] - if not(os.path.isdir("custom_msgs")): - os.mkdir("custom_msgs") if not(os.path.isdir(msgPkgPath)): os.mkdir(msgPkgPath) if not(os.path.isdir(msgPkgPath + "/msg")): @@ -48,10 +46,8 @@ def genDepMsgHeader(genMsg): template = env.get_template('header_template.tpl') datatext = template.render({ "msg": msg }) - msgPkgPath = "custom_msgs" + "/" + msg['pkg'] + msgPkgPath = msg['pkg'] - if not(os.path.isdir("custom_msgs")): - os.mkdir("custom_msgs") if not(os.path.isdir(msgPkgPath)): os.mkdir(msgPkgPath) if not(os.path.isdir(msgPkgPath + "/msg")): diff --git a/mros2_header_generator/msg_data_generator.py b/mros2_header_generator/msg_data_generator.py index 3be8cf4..1bb758a 100644 --- a/mros2_header_generator/msg_data_generator.py +++ b/mros2_header_generator/msg_data_generator.py @@ -5,8 +5,8 @@ def msgDataGenerator(line): dependingFileNames = [] - if os.path.isfile("custom_msgs/" + line): - with open("custom_msgs/" + line, 'r') as m_f: + if os.path.isfile(line): + with open(line, 'r') as m_f: arr = m_f.readlines() msgDef = [] for m_line in arr: diff --git a/mros2_header_generator/msg_def_generator.py b/mros2_header_generator/msg_def_generator.py index 7afc03b..ada6008 100644 --- a/mros2_header_generator/msg_def_generator.py +++ b/mros2_header_generator/msg_def_generator.py @@ -68,7 +68,7 @@ def msgDefGenerator(msgDefStr, dependingFileNames): } else: - if os.path.isfile("custom_msgs/" + msgType + ".msg"): # when custom type + if os.path.isfile(msgType + ".msg"): # when custom type dependingFileName = toSnakeCase(msgType) + ".hpp" depFileArr = dependingFileName.split("/") if depFileArr[2][0] == "_": diff --git a/mros2_msgs/geometry_msgs/msg/Point.msg b/mros2_msgs/geometry_msgs/msg/Point.msg new file mode 100644 index 0000000..681257d --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/Point.msg @@ -0,0 +1,3 @@ +float64 x +float64 y +float64 z \ No newline at end of file diff --git a/mros2_msgs/geometry_msgs/msg/Pose.msg b/mros2_msgs/geometry_msgs/msg/Pose.msg new file mode 100644 index 0000000..21eed4b --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/Pose.msg @@ -0,0 +1,2 @@ +geometry_msgs/msg/Point position +geometry_msgs/msg/Quaternion orientation \ No newline at end of file diff --git a/mros2_msgs/geometry_msgs/msg/Quaternion.msg b/mros2_msgs/geometry_msgs/msg/Quaternion.msg new file mode 100644 index 0000000..e6c4bed --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/Quaternion.msg @@ -0,0 +1,4 @@ +float64 x +float64 y +float64 z +float64 w \ No newline at end of file diff --git a/mros2_msgs/geometry_msgs/msg/Twist.msg b/mros2_msgs/geometry_msgs/msg/Twist.msg new file mode 100644 index 0000000..a888447 --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/Twist.msg @@ -0,0 +1,2 @@ +geometry_msgs/msg/Vector3 linear +geometry_msgs/msg/Vector3 angular \ No newline at end of file diff --git a/mros2_msgs/geometry_msgs/msg/Vector3.msg b/mros2_msgs/geometry_msgs/msg/Vector3.msg new file mode 100644 index 0000000..681257d --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/Vector3.msg @@ -0,0 +1,3 @@ +float64 x +float64 y +float64 z \ No newline at end of file diff --git a/mros2_msgs/geometry_msgs/msg/point.hpp b/mros2_msgs/geometry_msgs/msg/point.hpp new file mode 100644 index 0000000..a3fb695 --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/point.hpp @@ -0,0 +1,266 @@ +#ifndef _GEOMETRY_MSGS_MSG_POINT_H +#define _GEOMETRY_MSGS_MSG_POINT_H + +#include +#include + +using namespace std; + +namespace geometry_msgs +{ +namespace msg +{ +class Point +{ +public: + uint32_t cntPub = 0; + uint32_t cntSub = 0; + uint32_t idxSerialized = 0; + + typedef std::pair FragCopyReturnType; + + template + 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 + 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; + + double y; + + double z; + + + uint32_t copyToBuf(uint8_t *addrPtr) + { + uint32_t tmpPub = 0; + uint32_t arraySize; + uint32_t stringSize; + + + + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + + memcpy(addrPtr,&x,8); + addrPtr += 8; + cntPub += 8; + + + + + + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + + memcpy(addrPtr,&y,8); + addrPtr += 8; + cntPub += 8; + + + + + + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + + memcpy(addrPtr,&z,8); + addrPtr += 8; + cntPub += 8; + + + + + return cntPub; + } + + uint32_t copyFromBuf(const uint8_t *addrPtr) { + uint32_t tmpSub = 0; + uint32_t arraySize; + uint32_t stringSize; + + + + + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + addrPtr += 1; + } + cntSub += 8-(cntSub%8); + } + + memcpy(&x,addrPtr,8); + addrPtr += 8; + cntSub += 8; + + + + + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + addrPtr += 1; + } + cntSub += 8-(cntSub%8); + } + + memcpy(&y,addrPtr,8); + addrPtr += 8; + cntSub += 8; + + + + + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + addrPtr += 1; + } + cntSub += 8-(cntSub%8); + } + + memcpy(&z,addrPtr,8); + addrPtr += 8; + cntSub += 8; + + + + return cntSub; + } + + 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; + } + + uint32_t getTotalSize(){ + uint32_t tmpCntPub = cntPub; + cntPub = 0; + 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"; +}; +}; +} + +namespace message_traits +{ +template<> +struct TypeName { + static const char* value() + { + return "geometry_msgs::msg::dds_::Point_"; + } +}; +} + +#endif \ No newline at end of file diff --git a/mros2_msgs/geometry_msgs/msg/pose.hpp b/mros2_msgs/geometry_msgs/msg/pose.hpp new file mode 100644 index 0000000..35530b1 --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/pose.hpp @@ -0,0 +1,210 @@ +#ifndef _GEOMETRY_MSGS_MSG_POSE_H +#define _GEOMETRY_MSGS_MSG_POSE_H + +#include +#include +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/quaternion.hpp" + +using namespace std; + +namespace geometry_msgs +{ +namespace msg +{ +class Pose +{ +public: + uint32_t cntPub = 0; + uint32_t cntSub = 0; + uint32_t idxSerialized = 0; + + typedef std::pair FragCopyReturnType; + + template + 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 + 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; + + geometry_msgs::msg::Quaternion orientation; + + + uint32_t copyToBuf(uint8_t *addrPtr) + { + uint32_t tmpPub = 0; + uint32_t arraySize; + uint32_t stringSize; + + + + tmpPub = position.copyToBuf(addrPtr); + cntPub += tmpPub; + addrPtr += tmpPub; + + + + + + tmpPub = orientation.copyToBuf(addrPtr); + cntPub += tmpPub; + addrPtr += tmpPub; + + + + + return cntPub; + } + + uint32_t copyFromBuf(const uint8_t *addrPtr) { + uint32_t tmpSub = 0; + uint32_t arraySize; + uint32_t stringSize; + + + + + tmpSub = position.copyFromBuf(addrPtr); + cntSub += tmpSub; + addrPtr += tmpSub; + + + + + + + tmpSub = orientation.copyFromBuf(addrPtr); + cntSub += tmpSub; + addrPtr += tmpSub; + + + + + + return cntSub; + } + + 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; + } + + uint32_t getTotalSize(){ + uint32_t tmpCntPub = cntPub; + cntPub = 0; + 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"; +}; +}; +} + +namespace message_traits +{ +template<> +struct TypeName { + static const char* value() + { + return "geometry_msgs::msg::dds_::Pose_"; + } +}; +} + +#endif \ No newline at end of file diff --git a/mros2_msgs/geometry_msgs/msg/quaternion.hpp b/mros2_msgs/geometry_msgs/msg/quaternion.hpp new file mode 100644 index 0000000..afea149 --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/quaternion.hpp @@ -0,0 +1,298 @@ +#ifndef _GEOMETRY_MSGS_MSG_QUATERNION_H +#define _GEOMETRY_MSGS_MSG_QUATERNION_H + +#include +#include + +using namespace std; + +namespace geometry_msgs +{ +namespace msg +{ +class Quaternion +{ +public: + uint32_t cntPub = 0; + uint32_t cntSub = 0; + uint32_t idxSerialized = 0; + + typedef std::pair FragCopyReturnType; + + template + 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 + 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; + + double y; + + double z; + + double w; + + + uint32_t copyToBuf(uint8_t *addrPtr) + { + uint32_t tmpPub = 0; + uint32_t arraySize; + uint32_t stringSize; + + + + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + + memcpy(addrPtr,&x,8); + addrPtr += 8; + cntPub += 8; + + + + + + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + + memcpy(addrPtr,&y,8); + addrPtr += 8; + cntPub += 8; + + + + + + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + + memcpy(addrPtr,&z,8); + addrPtr += 8; + cntPub += 8; + + + + + + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + + memcpy(addrPtr,&w,8); + addrPtr += 8; + cntPub += 8; + + + + + return cntPub; + } + + uint32_t copyFromBuf(const uint8_t *addrPtr) { + uint32_t tmpSub = 0; + uint32_t arraySize; + uint32_t stringSize; + + + + + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + addrPtr += 1; + } + cntSub += 8-(cntSub%8); + } + + memcpy(&x,addrPtr,8); + addrPtr += 8; + cntSub += 8; + + + + + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + addrPtr += 1; + } + cntSub += 8-(cntSub%8); + } + + memcpy(&y,addrPtr,8); + addrPtr += 8; + cntSub += 8; + + + + + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + addrPtr += 1; + } + cntSub += 8-(cntSub%8); + } + + memcpy(&z,addrPtr,8); + addrPtr += 8; + cntSub += 8; + + + + + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + addrPtr += 1; + } + cntSub += 8-(cntSub%8); + } + + memcpy(&w,addrPtr,8); + addrPtr += 8; + cntSub += 8; + + + + return cntSub; + } + + 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; + } + + uint32_t getTotalSize(){ + uint32_t tmpCntPub = cntPub; + cntPub = 0; + 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"; +}; +}; +} + +namespace message_traits +{ +template<> +struct TypeName { + static const char* value() + { + return "geometry_msgs::msg::dds_::Quaternion_"; + } +}; +} + +#endif \ No newline at end of file diff --git a/mros2_msgs/geometry_msgs/msg/twist.hpp b/mros2_msgs/geometry_msgs/msg/twist.hpp new file mode 100644 index 0000000..478cbf4 --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/twist.hpp @@ -0,0 +1,210 @@ +#ifndef _GEOMETRY_MSGS_MSG_TWIST_H +#define _GEOMETRY_MSGS_MSG_TWIST_H + +#include +#include +#include "geometry_msgs/msg/vector3.hpp" +#include "geometry_msgs/msg/vector3.hpp" + +using namespace std; + +namespace geometry_msgs +{ +namespace msg +{ +class Twist +{ +public: + uint32_t cntPub = 0; + uint32_t cntSub = 0; + uint32_t idxSerialized = 0; + + typedef std::pair FragCopyReturnType; + + template + 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 + 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::Vector3 linear; + + geometry_msgs::msg::Vector3 angular; + + + uint32_t copyToBuf(uint8_t *addrPtr) + { + uint32_t tmpPub = 0; + uint32_t arraySize; + uint32_t stringSize; + + + + tmpPub = linear.copyToBuf(addrPtr); + cntPub += tmpPub; + addrPtr += tmpPub; + + + + + + tmpPub = angular.copyToBuf(addrPtr); + cntPub += tmpPub; + addrPtr += tmpPub; + + + + + return cntPub; + } + + uint32_t copyFromBuf(const uint8_t *addrPtr) { + uint32_t tmpSub = 0; + uint32_t arraySize; + uint32_t stringSize; + + + + + tmpSub = linear.copyFromBuf(addrPtr); + cntSub += tmpSub; + addrPtr += tmpSub; + + + + + + + tmpSub = angular.copyFromBuf(addrPtr); + cntSub += tmpSub; + addrPtr += tmpSub; + + + + + + return cntSub; + } + + 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; + } + + uint32_t getTotalSize(){ + uint32_t tmpCntPub = cntPub; + cntPub = 0; + 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_::Twist"; +}; +}; +} + +namespace message_traits +{ +template<> +struct TypeName { + static const char* value() + { + return "geometry_msgs::msg::dds_::Twist_"; + } +}; +} + +#endif \ No newline at end of file diff --git a/mros2_msgs/geometry_msgs/msg/vector3.hpp b/mros2_msgs/geometry_msgs/msg/vector3.hpp new file mode 100644 index 0000000..291ebee --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/vector3.hpp @@ -0,0 +1,266 @@ +#ifndef _GEOMETRY_MSGS_MSG_VECTOR3_H +#define _GEOMETRY_MSGS_MSG_VECTOR3_H + +#include +#include + +using namespace std; + +namespace geometry_msgs +{ +namespace msg +{ +class Vector3 +{ +public: + uint32_t cntPub = 0; + uint32_t cntSub = 0; + uint32_t idxSerialized = 0; + + typedef std::pair FragCopyReturnType; + + template + 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 + 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; + + double y; + + double z; + + + uint32_t copyToBuf(uint8_t *addrPtr) + { + uint32_t tmpPub = 0; + uint32_t arraySize; + uint32_t stringSize; + + + + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + + memcpy(addrPtr,&x,8); + addrPtr += 8; + cntPub += 8; + + + + + + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + + memcpy(addrPtr,&y,8); + addrPtr += 8; + cntPub += 8; + + + + + + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + + memcpy(addrPtr,&z,8); + addrPtr += 8; + cntPub += 8; + + + + + return cntPub; + } + + uint32_t copyFromBuf(const uint8_t *addrPtr) { + uint32_t tmpSub = 0; + uint32_t arraySize; + uint32_t stringSize; + + + + + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + addrPtr += 1; + } + cntSub += 8-(cntSub%8); + } + + memcpy(&x,addrPtr,8); + addrPtr += 8; + cntSub += 8; + + + + + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + addrPtr += 1; + } + cntSub += 8-(cntSub%8); + } + + memcpy(&y,addrPtr,8); + addrPtr += 8; + cntSub += 8; + + + + + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + addrPtr += 1; + } + cntSub += 8-(cntSub%8); + } + + memcpy(&z,addrPtr,8); + addrPtr += 8; + cntSub += 8; + + + + return cntSub; + } + + 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; + } + + uint32_t getTotalSize(){ + uint32_t tmpCntPub = cntPub; + cntPub = 0; + 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_::Vector3"; +}; +}; +} + +namespace message_traits +{ +template<> +struct TypeName { + static const char* value() + { + return "geometry_msgs::msg::dds_::Vector3_"; + } +}; +} + +#endif \ No newline at end of file