From c40b4826fbab91636739e08bee2cc5a5d53725e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 9 Dec 2021 18:00:20 +0900 Subject: [PATCH 01/70] header_generator for simple custom created --- include/mros2.h | 1 - mros2_header_generator/header_generator.py | 69 +++++++++++ mros2_header_generator/header_template.tpl | 115 +++++++++++++++++++ mros2_header_generator/msg_data_generator.py | 24 ++++ mros2_header_generator/msg_def_generator.py | 56 +++++++++ src/mros2.cpp | 18 +-- 6 files changed, 275 insertions(+), 8 deletions(-) create mode 100644 mros2_header_generator/header_generator.py create mode 100644 mros2_header_generator/header_template.tpl create mode 100644 mros2_header_generator/msg_data_generator.py create mode 100644 mros2_header_generator/msg_def_generator.py diff --git a/include/mros2.h b/include/mros2.h index d9b6432..5e84424 100644 --- a/include/mros2.h +++ b/include/mros2.h @@ -70,7 +70,6 @@ class Subscriber { public: std::string topic_name; - template static void callback_handler( void* callee, const rtps::ReaderCacheChange& cacheChange diff --git a/mros2_header_generator/header_generator.py b/mros2_header_generator/header_generator.py new file mode 100644 index 0000000..55bfa16 --- /dev/null +++ b/mros2_header_generator/header_generator.py @@ -0,0 +1,69 @@ +# generate message type header file for mROS2 + +import os +import json +import sys +from jinja2 import Environment, FileSystemLoader +from msg_data_generator import msgDataGenerator + +# standard types for ROS2 +stdMsgs = { + "std_msgs/msg/int8.hpp": 1, + "std_msgs/msg/u_int8.hpp": 2, + "std_msgs/msg/int16.hpp": 3, + "std_msgs/msg/u_int16.hpp": 4, + "std_msgs/msg/int32.hpp": 5, + "std_msgs/msg/u_int32.hpp": 6, + "std_msgs/msg/int64.hpp": 7, + "std_msgs/msg/u_int64.hpp": 8, + "std_msgs/msg/float32.hpp": 9, + "std_msgs/msg/float64.hpp": 10, + "std_msgs/msg/string.hpp": 11, + "std_msgs/msg/bool.hpp": 12, + "std_msgs/msg/byte.hpp": 13, +} + +msgs = [] +depMsgs = [] +includedStdMsgs = [] + +arg = sys.argv +if(len(arg) == 1): + raise Exception('Error: json file is not specified') +jsonFileName = arg[1] + +appDir = '../../mros2' +msgIncludePath = appDir + "/" + "mros2_msgs" + "/" +fileDir = os.getcwd() + +def main(): + #if not(os.path.isdir(msgIncludePath)): + #os.mkdir(msgIncludePath) + if not(os.path.isfile(fileDir + "/" + jsonFileName)): + raise Exception('specified json file (' + fileDir + "/" + jsonFileName + ') not found') + + with open(fileDir + "/" + jsonFileName, 'r') as f: + jsonData = json.load(f) + for line in jsonData['includingMsgs']: + line = line.strip() + print(line) + if line not in stdMsgs: # when custom type + msgs.append(msgDataGenerator(line)) # generate message data of the custom type + + #os.chdir(fileDir) + + # generate header file for mros2 + for msg in msgs: + env = Environment(loader=FileSystemLoader(appDir + '/mros2_header_generator')) + template = env.get_template('header_template.tpl') + datatext = template.render({"msg": msg}) + print(datatext) + msgPkgPath = msgIncludePath + msg['pkg'] + if not(os.path.isdir(msgPkgPath)): + os.mkdir(msgPkgPath) + os.mkdir(msgPkgPath + "/msg") + with open(os.path.join(msgPkgPath, "msg", msg['name'].lower() + ".hpp"), "wb") as f: + f.write(datatext.encode('utf-8')) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl new file mode 100644 index 0000000..71bc483 --- /dev/null +++ b/mros2_header_generator/header_template.tpl @@ -0,0 +1,115 @@ +#ifndef _{{msg.PKG}}_MSG_{{msg.NAME}}_H +#define _{{msg.PKG}}_MSG_{{msg.NAME}}_H + +#include +#include +#include + +using namespace std; + +namespace {{msg.pkg}} +{ +namespace msg +{ +class {{msg.name}} +{ +public: + {%for def_data in msg.def %} + {%if def_data.isArray %}std::vector<{{def_data.cppType}}>{% else %}{{def_data.cppType}}{% endif %} {{def_data.typeName}}; + {% endfor %} + + uint8_t getTotalSize(){ + return ({%for def_data in msg.def %} + {%if def_data.isArray %} + 4+{{def_data.size}}*({{def_data.typeName}}.size()) + + + {%elif def_data.cppType=="string"%} + 5+{{def_data.typeName}}.size() + + + {%else%} + {{def_data.size}} + + + {%endif%} + {%endfor%} + 0); + } + + void copyToBuf(uint8_t *addrPtr) + { + {%for def_data in msg.def %} + + {% if def_data.isArray%}{ + uint32_t arraySize = {{def_data.typeName}}.size(); + memcpy(addrPtr,&arraySize,4); + addrPtr += 4; + const {{def_data.cppType}}* ptr = {{def_data.typeName}}.data(); + for(int i=0; i +struct TypeName<{{msg.pkg}}::msg::{{msg.name}}*> { + static const char* value() + { + return "{{msg.pkg}}::msg::dds_::{{msg.name}}_"; + } +}; +} + +#endif \ No newline at end of file diff --git a/mros2_header_generator/msg_data_generator.py b/mros2_header_generator/msg_data_generator.py new file mode 100644 index 0000000..57716b9 --- /dev/null +++ b/mros2_header_generator/msg_data_generator.py @@ -0,0 +1,24 @@ +import os +from msg_def_generator import msgDefGenerator + +# generate message data is mainly about the name, definition of the type + +def msgDataGenerator(line): + if os.path.isfile(line): + with open(line, 'r') as m_f: + arr = m_f.readlines() + msgDef = [] + for i, m_line in enumerate(arr): + msgDef.append(msgDefGenerator(m_line)) + + lineArr = line.strip().split('/') + lineArr[2] = lineArr[2].rstrip('.msg') + return { + 'name': lineArr[2], + 'pkg': lineArr[0], + 'NAME': lineArr[2].upper(), + 'PKG': lineArr[0].upper(), + 'def': msgDef, + } + else: + raise Exception('msg header file "' + line + '" not found.') \ No newline at end of file diff --git a/mros2_header_generator/msg_def_generator.py b/mros2_header_generator/msg_def_generator.py new file mode 100644 index 0000000..4c181ec --- /dev/null +++ b/mros2_header_generator/msg_def_generator.py @@ -0,0 +1,56 @@ +# mapping between ros2Type to cppType +msgCppTypes = { + "int8": "int8_t", + "uint8": "uint8_t", + "int16": "int16_t", + "uint16": "uint16_t", + "int32": "int32_t", + "uint32": "uint32_t", + "int64": "int64_t", + "uint64": "uint64_t", + "float32": "float", + "float64": "double", + "string": "string", + "bool": "uint8_t", + "byte": "uint8_t", +} + +# size of each type +msgSizes = { + "string": 1, "bool": 1, + "int8": 1, "uint8": 1, + "int16": 2, "uint16": 2, + "int32": 4, "uint32": 4, + "int64": 8, "uint64": 8, + "float32": 4, "float64": 8 +} + +# generate detail data of type definition + + +def msgDefGenerator(msgDefStr): + + # split the type def and name of the type ('string name' -> ['string', 'name']) + msgDefArr = msgDefStr.split(' ') + print(msgDefArr) + msgType = msgDefArr[0] # each type (ex. string, int8, float32, ...) + msgName = msgDefArr[1] # name of each type (ex. name, height, weight, ...) + isArray = False + + # when array (ex. int8[], float32[], ...)msgType = msgType[:-2] + if msgType[-2:] == "[]": + isArray = True + msgType = msgType[:-2] + + if msgType in msgCppTypes: # when primitive type of ROS2 + return { + 'rosType': msgType, + 'cppType': msgCppTypes[msgType], + 'typeName': msgName, + 'size': msgSizes[msgType], + 'isArray': isArray, + 'isCustomType': False + } + + else: + print('type is not found') \ No newline at end of file diff --git a/src/mros2.cpp b/src/mros2.cpp index 97e73b9..e847d04 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -24,6 +24,7 @@ #include "std_msgs/msg/u_int64.hpp" #include "std_msgs/msg/w_string.hpp" +#include "std_msgs/msg/health.hpp" #include "TEST.hpp" #ifndef __MBED__ @@ -222,7 +223,7 @@ Subscriber Node::create_subscription(std::string topic_name, int qos, void (*fp) data_p = new SubscribeDataType; data_p->cb_fp = (void (*)(intptr_t))fp; data_p->argp = (intptr_t)NULL; - reader->registerCallback(sub.callback_handler, (void *)data_p); + reader->registerCallback(sub.callback_handler, (void *)data_p); /* Register callback to ensure that a subscriber is matched to the reader before receiving messages */ part_ptr->registerOnNewPublisherMatchedCallback(subMatch, &pubMatched); @@ -231,10 +232,9 @@ Subscriber Node::create_subscription(std::string topic_name, int qos, void (*fp) return sub; } -template void Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange) { - T msg; + std_msgs::msg::Health msg; msg.copyFromBuf(&cacheChange.data[4]); SubscribeDataType *sub = (SubscribeDataType *)callee; @@ -293,7 +293,7 @@ void setTrue(void *args) /* * specialize template functions */ -/* + template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Bool *)); template void mros2::Publisher::publish(std_msgs::msg::Bool &msg); @@ -312,13 +312,13 @@ template void mros2::Publisher::publish(std_msgs::msg::Float64 &msg); template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Int8 *)); -template void mros2::Publisher::publish(std_msgs::msg::Int8 &msg);*/ +template void mros2::Publisher::publish(std_msgs::msg::Int8 &msg); template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Int16 *)); template void mros2::Publisher::publish(std_msgs::msg::Int16 &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); -/* + template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Int32 *)); template void mros2::Publisher::publish(std_msgs::msg::Int32 &msg); @@ -349,7 +349,11 @@ template void mros2::Publisher::publish(std_msgs::msg::UInt64 &msg); template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::WString *)); -template void mros2::Publisher::publish(std_msgs::msg::WString &msg);*/ +template void mros2::Publisher::publish(std_msgs::msg::WString &msg); + +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Health *)); +template void mros2::Publisher::publish(std_msgs::msg::Health &msg); // Work in Progress: for custom message //template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); From 0d2007b34901712639bce765a9567d5578a40c7d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 9 Dec 2021 18:17:34 +0900 Subject: [PATCH 02/70] fixed mros2.cpp for health_pubsub --- src/mros2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mros2.cpp b/src/mros2.cpp index e847d04..4fada45 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -24,7 +24,7 @@ #include "std_msgs/msg/u_int64.hpp" #include "std_msgs/msg/w_string.hpp" -#include "std_msgs/msg/health.hpp" +#include "health_msgs/msg/health.hpp" #include "TEST.hpp" #ifndef __MBED__ From c4efb50799348fef9f942360d6227888fe18e582 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 9 Dec 2021 18:21:18 +0900 Subject: [PATCH 03/70] fixed mros2.cpp for health_pubsub --- mros2_header_generator/header_template.tpl | 3 +-- src/mros2.cpp | 6 +++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 71bc483..3a162b9 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -83,9 +83,8 @@ public: uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; - {{def_data.typeName}}.resize(stringSize+1); + {{def_data.typeName}}.resize(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); - {{def_data.typeName}}[stringSize] = '\0'; rbuf += stringSize; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); diff --git a/src/mros2.cpp b/src/mros2.cpp index 4fada45..9fa6dbb 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -351,9 +351,9 @@ template mros2::Publisher mros2::Node::create_publisher( template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::WString *)); template void mros2::Publisher::publish(std_msgs::msg::WString &msg); -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Health *)); -template void mros2::Publisher::publish(std_msgs::msg::Health &msg); +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(health_msgs::msg::Health *)); +template void mros2::Publisher::publish(health_msgs::msg::Health &msg); // Work in Progress: for custom message //template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); From 70cb25003c364a8dde74db2f8a77ccaf91ff064e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 9 Dec 2021 18:31:41 +0900 Subject: [PATCH 04/70] fixed header_template.tml for string --- mros2_header_generator/header_template.tpl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 3a162b9..cc629b6 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -54,7 +54,7 @@ public: memcpy(addrPtr,&stringSize,4); addrPtr += 4; memcpy(addrPtr,{{def_data.typeName}}.c_str(),stringSize); - addrPtr += stringSize; + addrPtr += stringSize+1; {% else %} memcpy(addrPtr,&{{def_data.typeName}},{{def_data.size}}); @@ -85,7 +85,7 @@ public: rbuf += 4; {{def_data.typeName}}.resize(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); - rbuf += stringSize; + rbuf += stringSize+1; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); rbuf += {{def_data.size}}; From 261de9de83e93a4d57b442331517b55c1d905d03 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 9 Dec 2021 18:33:49 +0900 Subject: [PATCH 05/70] fixed mros2.cpp for health_pubsub --- src/mros2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mros2.cpp b/src/mros2.cpp index 9fa6dbb..4f585a6 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -234,7 +234,7 @@ Subscriber Node::create_subscription(std::string topic_name, int qos, void (*fp) void Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange) { - std_msgs::msg::Health msg; + health_msgs::msg::Health msg; msg.copyFromBuf(&cacheChange.data[4]); SubscribeDataType *sub = (SubscribeDataType *)callee; From 7804f632925121edf5b494dae2980f2f25533ecd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 9 Dec 2021 18:35:53 +0900 Subject: [PATCH 06/70] fixed mros2.cpp for health_pubsub --- src/mros2.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/mros2.cpp b/src/mros2.cpp index 4f585a6..f07c12a 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -317,7 +317,6 @@ template void mros2::Publisher::publish(std_msgs::msg::Int8 &msg); template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Int16 *)); template void mros2::Publisher::publish(std_msgs::msg::Int16 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Int32 *)); From d88b2276dc1ad8d89e61ded5dd3adf49358a5831 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 9 Dec 2021 19:26:13 +0900 Subject: [PATCH 07/70] fixed header_template.tml for string --- mros2_header_generator/header_template.tpl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index cc629b6..4ce63d8 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -24,7 +24,7 @@ public: 4+{{def_data.size}}*({{def_data.typeName}}.size()) + {%elif def_data.cppType=="string"%} - 5+{{def_data.typeName}}.size() + 4+{{def_data.typeName}}.size() + {%else%} {{def_data.size}} @@ -54,7 +54,7 @@ public: memcpy(addrPtr,&stringSize,4); addrPtr += 4; memcpy(addrPtr,{{def_data.typeName}}.c_str(),stringSize); - addrPtr += stringSize+1; + addrPtr += stringSize; {% else %} memcpy(addrPtr,&{{def_data.typeName}},{{def_data.size}}); @@ -85,7 +85,7 @@ public: rbuf += 4; {{def_data.typeName}}.resize(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); - rbuf += stringSize+1; + rbuf += stringSize; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); rbuf += {{def_data.size}}; From 942dfa0e7b442287acdd3b2fb4d9fc5c3490f637 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 9 Dec 2021 19:54:12 +0900 Subject: [PATCH 08/70] fixed header_template.tml for string --- mros2_header_generator/header_template.tpl | 3 ++- mros2_msgs/std_msgs/msg/bool.hpp | 1 - mros2_msgs/std_msgs/msg/byte.hpp | 1 - mros2_msgs/std_msgs/msg/float32.hpp | 1 - mros2_msgs/std_msgs/msg/float64.hpp | 1 - mros2_msgs/std_msgs/msg/int16.hpp | 1 - mros2_msgs/std_msgs/msg/int32.hpp | 1 - mros2_msgs/std_msgs/msg/int64.hpp | 1 - mros2_msgs/std_msgs/msg/int8.hpp | 1 - mros2_msgs/std_msgs/msg/u_int16.hpp | 1 - mros2_msgs/std_msgs/msg/u_int32.hpp | 1 - mros2_msgs/std_msgs/msg/u_int64.hpp | 1 - mros2_msgs/std_msgs/msg/u_int8.hpp | 1 - 13 files changed, 2 insertions(+), 13 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 4ce63d8..2d397ae 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -24,7 +24,7 @@ public: 4+{{def_data.size}}*({{def_data.typeName}}.size()) + {%elif def_data.cppType=="string"%} - 4+{{def_data.typeName}}.size() + 5+{{def_data.typeName}}.size() + {%else%} {{def_data.size}} @@ -55,6 +55,7 @@ public: addrPtr += 4; memcpy(addrPtr,{{def_data.typeName}}.c_str(),stringSize); addrPtr += stringSize; + *addrPtr = 0; {% else %} memcpy(addrPtr,&{{def_data.typeName}},{{def_data.size}}); diff --git a/mros2_msgs/std_msgs/msg/bool.hpp b/mros2_msgs/std_msgs/msg/bool.hpp index fdddf01..ac4ffe1 100644 --- a/mros2_msgs/std_msgs/msg/bool.hpp +++ b/mros2_msgs/std_msgs/msg/bool.hpp @@ -13,7 +13,6 @@ class Bool { memcpy(addrPtr, &data, 1); addrPtr += 1; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) diff --git a/mros2_msgs/std_msgs/msg/byte.hpp b/mros2_msgs/std_msgs/msg/byte.hpp index 7c65664..c9b1bf4 100644 --- a/mros2_msgs/std_msgs/msg/byte.hpp +++ b/mros2_msgs/std_msgs/msg/byte.hpp @@ -13,7 +13,6 @@ class Byte { memcpy(addrPtr, &data, 1); addrPtr += 1; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) diff --git a/mros2_msgs/std_msgs/msg/float32.hpp b/mros2_msgs/std_msgs/msg/float32.hpp index e0e2d79..c248e08 100644 --- a/mros2_msgs/std_msgs/msg/float32.hpp +++ b/mros2_msgs/std_msgs/msg/float32.hpp @@ -13,7 +13,6 @@ class Float32 { memcpy(addrPtr, &data, 4); addrPtr += 4; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) diff --git a/mros2_msgs/std_msgs/msg/float64.hpp b/mros2_msgs/std_msgs/msg/float64.hpp index 0f7376e..eac3b10 100644 --- a/mros2_msgs/std_msgs/msg/float64.hpp +++ b/mros2_msgs/std_msgs/msg/float64.hpp @@ -13,7 +13,6 @@ class Float64 { memcpy(addrPtr, &data, 8); addrPtr += 8; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) diff --git a/mros2_msgs/std_msgs/msg/int16.hpp b/mros2_msgs/std_msgs/msg/int16.hpp index eed3797..0fa9b80 100644 --- a/mros2_msgs/std_msgs/msg/int16.hpp +++ b/mros2_msgs/std_msgs/msg/int16.hpp @@ -13,7 +13,6 @@ class Int16 { memcpy(addrPtr, &data, 2); addrPtr += 2; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) diff --git a/mros2_msgs/std_msgs/msg/int32.hpp b/mros2_msgs/std_msgs/msg/int32.hpp index 9cd6010..a3661ae 100644 --- a/mros2_msgs/std_msgs/msg/int32.hpp +++ b/mros2_msgs/std_msgs/msg/int32.hpp @@ -13,7 +13,6 @@ class Int32 { memcpy(addrPtr, &data, 4); addrPtr += 4; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) diff --git a/mros2_msgs/std_msgs/msg/int64.hpp b/mros2_msgs/std_msgs/msg/int64.hpp index a2dab34..182a657 100644 --- a/mros2_msgs/std_msgs/msg/int64.hpp +++ b/mros2_msgs/std_msgs/msg/int64.hpp @@ -13,7 +13,6 @@ class Int64 { memcpy(addrPtr, &data, 8); addrPtr += 8; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) diff --git a/mros2_msgs/std_msgs/msg/int8.hpp b/mros2_msgs/std_msgs/msg/int8.hpp index 4bb393d..15c98d4 100644 --- a/mros2_msgs/std_msgs/msg/int8.hpp +++ b/mros2_msgs/std_msgs/msg/int8.hpp @@ -13,7 +13,6 @@ class Int8 { memcpy(addrPtr, &data, 1); addrPtr += 1; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) diff --git a/mros2_msgs/std_msgs/msg/u_int16.hpp b/mros2_msgs/std_msgs/msg/u_int16.hpp index 56ca7e2..963d20b 100644 --- a/mros2_msgs/std_msgs/msg/u_int16.hpp +++ b/mros2_msgs/std_msgs/msg/u_int16.hpp @@ -13,7 +13,6 @@ class UInt16 { memcpy(addrPtr, &data, 2); addrPtr += 2; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) diff --git a/mros2_msgs/std_msgs/msg/u_int32.hpp b/mros2_msgs/std_msgs/msg/u_int32.hpp index 74aea48..6fb25e5 100644 --- a/mros2_msgs/std_msgs/msg/u_int32.hpp +++ b/mros2_msgs/std_msgs/msg/u_int32.hpp @@ -13,7 +13,6 @@ class UInt32 { memcpy(addrPtr, &data, 4); addrPtr += 4; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) diff --git a/mros2_msgs/std_msgs/msg/u_int64.hpp b/mros2_msgs/std_msgs/msg/u_int64.hpp index 5f7742f..f01e1a2 100644 --- a/mros2_msgs/std_msgs/msg/u_int64.hpp +++ b/mros2_msgs/std_msgs/msg/u_int64.hpp @@ -13,7 +13,6 @@ class UInt64 { memcpy(addrPtr, &data, 8); addrPtr += 8; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) diff --git a/mros2_msgs/std_msgs/msg/u_int8.hpp b/mros2_msgs/std_msgs/msg/u_int8.hpp index a59b3b8..119f3d3 100644 --- a/mros2_msgs/std_msgs/msg/u_int8.hpp +++ b/mros2_msgs/std_msgs/msg/u_int8.hpp @@ -13,7 +13,6 @@ class UInt8 { memcpy(addrPtr, &data, 1); addrPtr += 1; - *addrPtr = 0; } void copyFromBuf(const uint8_t *addrPtr) From 305374280cd6c0cccd4490e484f50f924ca99ee5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 9 Dec 2021 23:27:42 +0900 Subject: [PATCH 09/70] fixed header_template.tml for string --- mros2_header_generator/header_template.tpl | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 2d397ae..f1209f6 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -84,8 +84,10 @@ public: uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; - {{def_data.typeName}}.resize(stringSize); - memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); + char buf_char[stringSize+1]; + memcpy(&buf_char,rbuf,stringSize); + buf_char[stringSize] = '\0'; + {{def_data.typeName}} = buf_char; rbuf += stringSize; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); From 93fd5a8c8efa730dbe3054f5cfae31efd5ab1d91 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 9 Dec 2021 23:40:27 +0900 Subject: [PATCH 10/70] fixed header_template.tml for string --- mros2_header_generator/header_template.tpl | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index f1209f6..2d397ae 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -84,10 +84,8 @@ public: uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; - char buf_char[stringSize+1]; - memcpy(&buf_char,rbuf,stringSize); - buf_char[stringSize] = '\0'; - {{def_data.typeName}} = buf_char; + {{def_data.typeName}}.resize(stringSize); + memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); rbuf += stringSize; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); From 1e593c0fb8cb9bc29162ea6d9097f30210b5677b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Fri, 10 Dec 2021 09:21:08 +0900 Subject: [PATCH 11/70] mros2.cpp modified for location_pubsub --- src/mros2.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/mros2.cpp b/src/mros2.cpp index f07c12a..c502434 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -25,6 +25,7 @@ #include "std_msgs/msg/w_string.hpp" #include "health_msgs/msg/health.hpp" +#include "location_msgs/msg/location.hpp" #include "TEST.hpp" #ifndef __MBED__ @@ -234,7 +235,7 @@ Subscriber Node::create_subscription(std::string topic_name, int qos, void (*fp) void Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange) { - health_msgs::msg::Health msg; + location_msgs::msg::Location msg; msg.copyFromBuf(&cacheChange.data[4]); SubscribeDataType *sub = (SubscribeDataType *)callee; @@ -354,6 +355,11 @@ template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(location_msgs::msg::Location *)); +template void mros2::Publisher::publish(location_msgs::msg::Location &msg); + + // Work in Progress: for custom message //template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); //template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(TEST*)); From 01b9636718af59b2169c7dcf92e27deeab41a0b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sat, 11 Dec 2021 13:41:41 +0900 Subject: [PATCH 12/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 2 +- src/mros2.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 2d397ae..ae554c2 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -84,7 +84,7 @@ public: uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; - {{def_data.typeName}}.resize(stringSize); + {{def_data.typeName}}.reserve(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); rbuf += stringSize; {% else %} diff --git a/src/mros2.cpp b/src/mros2.cpp index c502434..b6f5742 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -25,7 +25,7 @@ #include "std_msgs/msg/w_string.hpp" #include "health_msgs/msg/health.hpp" -#include "location_msgs/msg/location.hpp" +//#include "location_msgs/msg/location.hpp" #include "TEST.hpp" #ifndef __MBED__ @@ -235,7 +235,7 @@ Subscriber Node::create_subscription(std::string topic_name, int qos, void (*fp) void Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange) { - location_msgs::msg::Location msg; + health_msgs::msg::Health msg; msg.copyFromBuf(&cacheChange.data[4]); SubscribeDataType *sub = (SubscribeDataType *)callee; @@ -355,9 +355,9 @@ template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +/*template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(location_msgs::msg::Location *)); -template void mros2::Publisher::publish(location_msgs::msg::Location &msg); +template void mros2::Publisher::publish(location_msgs::msg::Location &msg);*/ // Work in Progress: for custom message From 5250171cda7721b53374726c2576c0f8ab4e4edc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sat, 11 Dec 2021 14:27:19 +0900 Subject: [PATCH 13/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index ae554c2..2d397ae 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -84,7 +84,7 @@ public: uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; - {{def_data.typeName}}.reserve(stringSize); + {{def_data.typeName}}.resize(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); rbuf += stringSize; {% else %} From 1a8cf84facc334e7719e42e878cbf8d6b1e493fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sat, 11 Dec 2021 15:45:36 +0900 Subject: [PATCH 14/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 2d397ae..ba7d3b4 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -24,7 +24,7 @@ public: 4+{{def_data.size}}*({{def_data.typeName}}.size()) + {%elif def_data.cppType=="string"%} - 5+{{def_data.typeName}}.size() + 4+{{def_data.typeName}}.size() + {%else%} {{def_data.size}} From 85408fceaa15c5ed3f9a6a1182215291c1aafef2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sat, 11 Dec 2021 16:02:14 +0900 Subject: [PATCH 15/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index ba7d3b4..2d397ae 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -24,7 +24,7 @@ public: 4+{{def_data.size}}*({{def_data.typeName}}.size()) + {%elif def_data.cppType=="string"%} - 4+{{def_data.typeName}}.size() + 5+{{def_data.typeName}}.size() + {%else%} {{def_data.size}} From 7b620be1c84adbb6f588d4190b2ead3def926501 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sat, 11 Dec 2021 16:17:28 +0900 Subject: [PATCH 16/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 2d397ae..2d0b665 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -84,7 +84,7 @@ public: uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; - {{def_data.typeName}}.resize(stringSize); + {{def_data.typeName}}.resize(stringSize,''); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); rbuf += stringSize; {% else %} From ba5f0e11781b751f3f9bf1121d44766ed3ee8ff5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sat, 11 Dec 2021 16:18:54 +0900 Subject: [PATCH 17/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 2d0b665..779b08f 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -84,7 +84,7 @@ public: uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; - {{def_data.typeName}}.resize(stringSize,''); + {{def_data.typeName}}.resize(stringSize,' '); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); rbuf += stringSize; {% else %} From 0f556fc04c8275b93711c5c620b6a7db37152eb3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sat, 11 Dec 2021 16:20:21 +0900 Subject: [PATCH 18/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 779b08f..7c11bf0 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -24,7 +24,7 @@ public: 4+{{def_data.size}}*({{def_data.typeName}}.size()) + {%elif def_data.cppType=="string"%} - 5+{{def_data.typeName}}.size() + 5+{{def_data.typeName}}.capacity() + {%else%} {{def_data.size}} @@ -50,7 +50,7 @@ public: } {% elif def_data.cppType == "string"%} - uint32_t stringSize = {{def_data.typeName}}.size(); + uint32_t stringSize = {{def_data.typeName}}.capacity(); memcpy(addrPtr,&stringSize,4); addrPtr += 4; memcpy(addrPtr,{{def_data.typeName}}.c_str(),stringSize); @@ -84,7 +84,7 @@ public: uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; - {{def_data.typeName}}.resize(stringSize,' '); + {{def_data.typeName}}.resize(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); rbuf += stringSize; {% else %} From 0736f408829fcc96352dcbb91bdd8d53c39fc3bf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sat, 11 Dec 2021 23:47:15 +0900 Subject: [PATCH 19/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 7c11bf0..601e324 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -24,7 +24,7 @@ public: 4+{{def_data.size}}*({{def_data.typeName}}.size()) + {%elif def_data.cppType=="string"%} - 5+{{def_data.typeName}}.capacity() + 5+{{def_data.typeName}}.size() + {%else%} {{def_data.size}} @@ -50,7 +50,7 @@ public: } {% elif def_data.cppType == "string"%} - uint32_t stringSize = {{def_data.typeName}}.capacity(); + uint32_t stringSize = {{def_data.typeName}}.size(); memcpy(addrPtr,&stringSize,4); addrPtr += 4; memcpy(addrPtr,{{def_data.typeName}}.c_str(),stringSize); @@ -82,7 +82,10 @@ public: } {% elif def_data.cppType == "string"%} uint32_t stringSize; - memcpy(&stringSize, rbuf, 4); + char buf_char[stringSize+1]; + memcpy(&buf_char,rbuf,stringSize); + buf_char[stringSize] = '\0'; + {{def_data.typeName}} = buf_char; rbuf += 4; {{def_data.typeName}}.resize(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); From cd20961c950de918f7be77311c11ab717befa8ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sat, 11 Dec 2021 23:59:37 +0900 Subject: [PATCH 20/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 601e324..446fc43 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -82,9 +82,8 @@ public: } {% elif def_data.cppType == "string"%} uint32_t stringSize; - char buf_char[stringSize+1]; + std::string buf_char; memcpy(&buf_char,rbuf,stringSize); - buf_char[stringSize] = '\0'; {{def_data.typeName}} = buf_char; rbuf += 4; {{def_data.typeName}}.resize(stringSize); From 6c7113000f709cdcdadc987311a6418395c1ea6b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 12 Dec 2021 00:04:41 +0900 Subject: [PATCH 21/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 446fc43..14e4940 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -82,12 +82,11 @@ public: } {% elif def_data.cppType == "string"%} uint32_t stringSize; - std::string buf_char; - memcpy(&buf_char,rbuf,stringSize); - {{def_data.typeName}} = buf_char; + memcpy(&stringSize, rbuf, 4); rbuf += 4; - {{def_data.typeName}}.resize(stringSize); - memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); + std::string buf_char; + memcpy(buf_char.c_str(),rbuf,stringSize); + {{def_data.typeName}}=buf_char; rbuf += stringSize; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); From b948b9a51d0bcde73199babd81940c5682618765 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 12 Dec 2021 00:10:23 +0900 Subject: [PATCH 22/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 14e4940..7d89b53 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -85,7 +85,7 @@ public: memcpy(&stringSize, rbuf, 4); rbuf += 4; std::string buf_char; - memcpy(buf_char.c_str(),rbuf,stringSize); + memcpy(&buf_char[0],rbuf,stringSize); {{def_data.typeName}}=buf_char; rbuf += stringSize; {% else %} From 59dd4ef0abc6bd9f857886fa9d3a8850ca74039a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 12 Dec 2021 00:13:20 +0900 Subject: [PATCH 23/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 7d89b53..4acc7e5 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -85,7 +85,7 @@ public: memcpy(&stringSize, rbuf, 4); rbuf += 4; std::string buf_char; - memcpy(&buf_char[0],rbuf,stringSize); + memcpy(&buf_char[0],rbuf,stringSize+1); {{def_data.typeName}}=buf_char; rbuf += stringSize; {% else %} From 606c985334790e5eee4b86fe90a751034f76294e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 12 Dec 2021 00:15:39 +0900 Subject: [PATCH 24/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 4acc7e5..c0b2519 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -84,10 +84,9 @@ public: uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; - std::string buf_char; - memcpy(&buf_char[0],rbuf,stringSize+1); - {{def_data.typeName}}=buf_char; - rbuf += stringSize; + {{def_data.typeName}}.resize(2); + memcpy(&{{def_data.typeName}}[0],rbuf,2); + rbuf += 2; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); rbuf += {{def_data.size}}; From 238508af35155619f7d81ddbe9b8d8f4fd8c0ba2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 12 Dec 2021 00:18:42 +0900 Subject: [PATCH 25/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index c0b2519..5afa59e 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -83,10 +83,11 @@ public: {% elif def_data.cppType == "string"%} uint32_t stringSize; memcpy(&stringSize, rbuf, 4); + cout << stringSize << endl; rbuf += 4; - {{def_data.typeName}}.resize(2); - memcpy(&{{def_data.typeName}}[0],rbuf,2); - rbuf += 2; + {{def_data.typeName}}.resize(stringSize); + memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); + rbuf += stringSize; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); rbuf += {{def_data.size}}; From 5138c7f0c73e2e9efbf4622aaf9eb40a265fac14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 12 Dec 2021 00:23:20 +0900 Subject: [PATCH 26/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 5afa59e..bf1e830 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -83,11 +83,11 @@ public: {% elif def_data.cppType == "string"%} uint32_t stringSize; memcpy(&stringSize, rbuf, 4); - cout << stringSize << endl; - rbuf += 4; - {{def_data.typeName}}.resize(stringSize); - memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); - rbuf += stringSize; + if (stringSize==2){ + {{def_data.typeName}}.resize(stringSize); + memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); + } + rbuf += 6; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); rbuf += {{def_data.size}}; From d56e94cebb8c1dbc5b82b3d469b79c5d0b2615e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 12 Dec 2021 00:26:16 +0900 Subject: [PATCH 27/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index bf1e830..da4f1f3 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -84,10 +84,14 @@ public: uint32_t stringSize; memcpy(&stringSize, rbuf, 4); if (stringSize==2){ + rbuf += 4; {{def_data.typeName}}.resize(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); + rbuf += 2; + } + else { + rbuf += 6; } - rbuf += 6; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); rbuf += {{def_data.size}}; From 693318ece4d18962bbc76c10e8e7b11a2d2278bd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 12 Dec 2021 00:40:29 +0900 Subject: [PATCH 28/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index da4f1f3..f8b9282 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -83,15 +83,11 @@ public: {% elif def_data.cppType == "string"%} uint32_t stringSize; memcpy(&stringSize, rbuf, 4); - if (stringSize==2){ - rbuf += 4; - {{def_data.typeName}}.resize(stringSize); - memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); - rbuf += 2; - } - else { - rbuf += 6; - } + uint32_t size = stringSize; + rbuf += 4; + {{def_data.typeName}}.resize(size); + memcpy(&{{def_data.typeName}}[0],rbuf,size); + rbuf += size; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); rbuf += {{def_data.size}}; From ae02372b4e11817e9aa68959656dfa4e525aa52d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 12 Dec 2021 00:45:26 +0900 Subject: [PATCH 29/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index f8b9282..ca79416 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -83,11 +83,13 @@ public: {% elif def_data.cppType == "string"%} uint32_t stringSize; memcpy(&stringSize, rbuf, 4); - uint32_t size = stringSize; rbuf += 4; - {{def_data.typeName}}.resize(size); - memcpy(&{{def_data.typeName}}[0],rbuf,size); - rbuf += size; + if (stringSize % 4 == 2){ + stringSize += 1; + } + {{def_data.typeName}}.resize(stringSize); + memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); + rbuf += stringSize; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); rbuf += {{def_data.size}}; From 85ce0e9acfee9fe6fbd0a739fdf91ad9a90519b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 12 Dec 2021 11:10:45 +0900 Subject: [PATCH 30/70] tpl file modified for string --- mros2_header_generator/header_template.tpl | 3 --- 1 file changed, 3 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index ca79416..2d397ae 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -84,9 +84,6 @@ public: uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; - if (stringSize % 4 == 2){ - stringSize += 1; - } {{def_data.typeName}}.resize(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); rbuf += stringSize; From 5f4eb825659b1daff996cb5ed7a31551c21b8a0a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Wed, 15 Dec 2021 15:39:42 +0900 Subject: [PATCH 31/70] mros2.cpp mros2.h fixed --- src/mros2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mros2.cpp b/src/mros2.cpp index 1f8e8e1..ff453fd 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -379,5 +379,5 @@ template void mros2::Publisher::publish(TEST& msg); template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(health_msgs::msg::Health*)); -template void mros2::Publisher::publish(health_msgs::msg::Healthg &msg); +template void mros2::Publisher::publish(health_msgs::msg::Health &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); From 1817f99c0479dd437164ed06ab83c05e950142dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 12:57:14 +0900 Subject: [PATCH 32/70] location funcs declared --- src/mros2.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/mros2.cpp b/src/mros2.cpp index ff453fd..14c20bc 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -26,6 +26,7 @@ //#include "TEST.hpp" #include "health_msgs/msg/health.hpp" +#include "location_msgs/msg/location.hpp" #ifndef __MBED__ /* Statement to avoid link error */ @@ -381,3 +382,8 @@ template mros2::Publisher mros2::Node::create_publisher(void *callee, const rtps::ReaderCacheChange &cacheChange); + +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(location_msgs::msg::Location*)); +template void mros2::Publisher::publish(location_msgs::msg::Location &msg); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); From 9be7ba1b7fb8dd6b9de0bc53715e0fe38373ba35 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 13:42:46 +0900 Subject: [PATCH 33/70] tpl modified for debug --- mros2_header_generator/header_template.tpl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 2d397ae..5d21e54 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -86,10 +86,10 @@ public: rbuf += 4; {{def_data.typeName}}.resize(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); - rbuf += stringSize; + rbuf += stringSize+1; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); - rbuf += {{def_data.size}}; + rbuf += 4; {% endif %} {% endfor %} } From 9adb29253b7ead25fddce940aba75e3776a831ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 13:52:39 +0900 Subject: [PATCH 34/70] mros2.cpp modified for float_location --- mros2_header_generator/header_template.tpl | 2 +- src/mros2.cpp | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 5d21e54..c09fac9 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -89,7 +89,7 @@ public: rbuf += stringSize+1; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); - rbuf += 4; + rbuf += {{def_data.size}}; {% endif %} {% endfor %} } diff --git a/src/mros2.cpp b/src/mros2.cpp index 14c20bc..725c113 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -27,6 +27,7 @@ //#include "TEST.hpp" #include "health_msgs/msg/health.hpp" #include "location_msgs/msg/location.hpp" +#include "float_location_msgs/msg/float_location.hpp" #ifndef __MBED__ /* Statement to avoid link error */ @@ -387,3 +388,8 @@ template mros2::Publisher mros2::Node::create_publisher(void *callee, const rtps::ReaderCacheChange &cacheChange); + +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(float_location_msgs::msg::FloatLocation*)); +template void mros2::Publisher::publish(float_location_msgs::msg::FloatLocation &msg); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); \ No newline at end of file From 2d2f3ccae58c17720a15ce48558f02d654ee76f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 13:57:26 +0900 Subject: [PATCH 35/70] mros2.cpp modified for float_location --- src/mros2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mros2.cpp b/src/mros2.cpp index 725c113..d057118 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -27,7 +27,7 @@ //#include "TEST.hpp" #include "health_msgs/msg/health.hpp" #include "location_msgs/msg/location.hpp" -#include "float_location_msgs/msg/float_location.hpp" +#include "float_location_msgs/msg/floatlocation.hpp" #ifndef __MBED__ /* Statement to avoid link error */ From 83c9dbe6c93602fc1d36c0682402f532739c2661 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 14:15:20 +0900 Subject: [PATCH 36/70] camel_to_snake func added --- mros2_header_generator/header_generator.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/mros2_header_generator/header_generator.py b/mros2_header_generator/header_generator.py index 55bfa16..2d9fd9d 100644 --- a/mros2_header_generator/header_generator.py +++ b/mros2_header_generator/header_generator.py @@ -3,6 +3,7 @@ import os import json import sys +import re from jinja2 import Environment, FileSystemLoader from msg_data_generator import msgDataGenerator @@ -36,6 +37,9 @@ msgIncludePath = appDir + "/" + "mros2_msgs" + "/" fileDir = os.getcwd() +def camel_to_snake(s: str) -> str: + return re.sub("((?<=[a-z0-9])[A-Z]|(?!^)[A-Z](?=[a-z]))", r"_\1", s).lower() + def main(): #if not(os.path.isdir(msgIncludePath)): #os.mkdir(msgIncludePath) @@ -62,7 +66,7 @@ def main(): if not(os.path.isdir(msgPkgPath)): os.mkdir(msgPkgPath) os.mkdir(msgPkgPath + "/msg") - with open(os.path.join(msgPkgPath, "msg", msg['name'].lower() + ".hpp"), "wb") as f: + with open(os.path.join(msgPkgPath, "msg", camel_to_snake(msg['name']) + ".hpp"), "wb") as f: f.write(datatext.encode('utf-8')) if __name__ == "__main__": From 8d0c2873993eb430683824b207a922df9f19365a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 14:15:42 +0900 Subject: [PATCH 37/70] camel_to_snake func added --- src/mros2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mros2.cpp b/src/mros2.cpp index d057118..725c113 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -27,7 +27,7 @@ //#include "TEST.hpp" #include "health_msgs/msg/health.hpp" #include "location_msgs/msg/location.hpp" -#include "float_location_msgs/msg/floatlocation.hpp" +#include "float_location_msgs/msg/float_location.hpp" #ifndef __MBED__ /* Statement to avoid link error */ From 190f79cbcb4ede5e42521454173bc255d3ab0fa5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 14:43:13 +0900 Subject: [PATCH 38/70] camel_to_snake func added --- src/mros2.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/mros2.cpp b/src/mros2.cpp index 725c113..aa31699 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -27,7 +27,7 @@ //#include "TEST.hpp" #include "health_msgs/msg/health.hpp" #include "location_msgs/msg/location.hpp" -#include "float_location_msgs/msg/float_location.hpp" +#include "flocation_msgs/msg/flocation.hpp" #ifndef __MBED__ /* Statement to avoid link error */ @@ -389,7 +389,7 @@ template mros2::Subscriber mros2::Node::create_subscription(std::string topic_na template void mros2::Publisher::publish(location_msgs::msg::Location &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(float_location_msgs::msg::FloatLocation*)); -template void mros2::Publisher::publish(float_location_msgs::msg::FloatLocation &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); \ No newline at end of file +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(flocation_msgs::msg::Flocation*)); +template void mros2::Publisher::publish(flocation_msgs::msg::Flocation &msg); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); \ No newline at end of file From a11a8dad0597db6a142023649c232c7a1b841d47 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 15:14:11 +0900 Subject: [PATCH 39/70] camel_to_snake func commeted out --- mros2_header_generator/header_generator.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mros2_header_generator/header_generator.py b/mros2_header_generator/header_generator.py index 2d9fd9d..6838b7d 100644 --- a/mros2_header_generator/header_generator.py +++ b/mros2_header_generator/header_generator.py @@ -37,8 +37,8 @@ msgIncludePath = appDir + "/" + "mros2_msgs" + "/" fileDir = os.getcwd() -def camel_to_snake(s: str) -> str: - return re.sub("((?<=[a-z0-9])[A-Z]|(?!^)[A-Z](?=[a-z]))", r"_\1", s).lower() +#def camel_to_snake(s: str) -> str: + #return re.sub("((?<=[a-z0-9])[A-Z]|(?!^)[A-Z](?=[a-z]))", r"_\1", s).lower() def main(): #if not(os.path.isdir(msgIncludePath)): @@ -66,7 +66,7 @@ def main(): if not(os.path.isdir(msgPkgPath)): os.mkdir(msgPkgPath) os.mkdir(msgPkgPath + "/msg") - with open(os.path.join(msgPkgPath, "msg", camel_to_snake(msg['name']) + ".hpp"), "wb") as f: + with open(os.path.join(msgPkgPath, "msg", msg['name'].islower() + ".hpp"), "wb") as f: f.write(datatext.encode('utf-8')) if __name__ == "__main__": From b1bd2c7c6704d671d575d5f9b84b2ff30747eb95 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 15:17:02 +0900 Subject: [PATCH 40/70] camel_to_snake func deleted --- mros2_header_generator/header_generator.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/mros2_header_generator/header_generator.py b/mros2_header_generator/header_generator.py index 6838b7d..55bfa16 100644 --- a/mros2_header_generator/header_generator.py +++ b/mros2_header_generator/header_generator.py @@ -3,7 +3,6 @@ import os import json import sys -import re from jinja2 import Environment, FileSystemLoader from msg_data_generator import msgDataGenerator @@ -37,9 +36,6 @@ msgIncludePath = appDir + "/" + "mros2_msgs" + "/" fileDir = os.getcwd() -#def camel_to_snake(s: str) -> str: - #return re.sub("((?<=[a-z0-9])[A-Z]|(?!^)[A-Z](?=[a-z]))", r"_\1", s).lower() - def main(): #if not(os.path.isdir(msgIncludePath)): #os.mkdir(msgIncludePath) @@ -66,7 +62,7 @@ def main(): if not(os.path.isdir(msgPkgPath)): os.mkdir(msgPkgPath) os.mkdir(msgPkgPath + "/msg") - with open(os.path.join(msgPkgPath, "msg", msg['name'].islower() + ".hpp"), "wb") as f: + with open(os.path.join(msgPkgPath, "msg", msg['name'].lower() + ".hpp"), "wb") as f: f.write(datatext.encode('utf-8')) if __name__ == "__main__": From 187dc51b7be19bb56207be76764bca696c3cb627 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 17:58:57 +0900 Subject: [PATCH 41/70] merge from main branch --- mros2_header_generator/header_template.tpl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index c09fac9..7990537 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -54,7 +54,7 @@ public: memcpy(addrPtr,&stringSize,4); addrPtr += 4; memcpy(addrPtr,{{def_data.typeName}}.c_str(),stringSize); - addrPtr += stringSize; + addrPtr += stringSize+1; *addrPtr = 0; {% else %} From 40c3adf9038362a4eb9e909eb1f523572be8a8cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 18:05:24 +0900 Subject: [PATCH 42/70] tmp modified for debug --- mros2_header_generator/header_template.tpl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 7990537..ee37e01 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -27,7 +27,7 @@ public: 5+{{def_data.typeName}}.size() + {%else%} - {{def_data.size}} + 4 + {%endif%} {%endfor%} @@ -89,7 +89,7 @@ public: rbuf += stringSize+1; {% else %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); - rbuf += {{def_data.size}}; + rbuf += 4; {% endif %} {% endfor %} } From 9cd79318ef25eb54dbf1c69e4b9da2369c30c1f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 16 Dec 2021 18:08:43 +0900 Subject: [PATCH 43/70] tmp modified for debug --- mros2_header_generator/header_template.tpl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index ee37e01..f143316 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -59,7 +59,7 @@ public: {% else %} memcpy(addrPtr,&{{def_data.typeName}},{{def_data.size}}); - addrPtr += {{def_data.size}}; + addrPtr += 4; {% endif %} From 133879966ac04363b60cdacc6daf9979315f8a08 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sat, 18 Dec 2021 16:59:50 +0900 Subject: [PATCH 44/70] header_template.tpl modified for processing memory alignment and successfully works (for simple_custom msg pub/sub) as far as I know --- mros2_header_generator/header_template.tpl | 109 ++++++++++++--------- 1 file changed, 64 insertions(+), 45 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index f143316..84d1229 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -14,86 +14,105 @@ namespace msg class {{msg.name}} { public: + uint8_t cntPub = 0; + uint8_t cntSub = 0; {%for def_data in msg.def %} - {%if def_data.isArray %}std::vector<{{def_data.cppType}}>{% else %}{{def_data.cppType}}{% endif %} {{def_data.typeName}}; + {{def_data.cppType}} {{def_data.typeName}}; {% endfor %} - uint8_t getTotalSize(){ - return ({%for def_data in msg.def %} - {%if def_data.isArray %} - 4+{{def_data.size}}*({{def_data.typeName}}.size()) - + - {%elif def_data.cppType=="string"%} - 5+{{def_data.typeName}}.size() - + - {%else%} - 4 - + - {%endif%} - {%endfor%} - 0); - } - void copyToBuf(uint8_t *addrPtr) { {%for def_data in msg.def %} - - {% if def_data.isArray%}{ - uint32_t arraySize = {{def_data.typeName}}.size(); - memcpy(addrPtr,&arraySize,4); - addrPtr += 4; - const {{def_data.cppType}}* ptr = {{def_data.typeName}}.data(); - for(int i=0; i0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; } + cntPub += 4-(cntPub%4); } - - {% elif def_data.cppType == "string"%} uint32_t stringSize = {{def_data.typeName}}.size(); memcpy(addrPtr,&stringSize,4); addrPtr += 4; + cntPub += 4; memcpy(addrPtr,{{def_data.typeName}}.c_str(),stringSize); - addrPtr += stringSize+1; - *addrPtr = 0; + addrPtr += stringSize; + cntPub += stringSize; {% else %} + if (cntPub%4 >0 && {{def_data.size}} > 1){ + if (({{def_data.size}} <= (4-(cntPub%4)))&&({{def_data.size}}==2)){ + for (int i=0; i<(4-(cntPub%4))-{{def_data.size}}; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-{{def_data.size}}; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } memcpy(addrPtr,&{{def_data.typeName}},{{def_data.size}}); - addrPtr += 4; + addrPtr += {{def_data.size}}; + cntPub += {{def_data.size}}; {% endif %} - {% endfor %} + + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } } void copyFromBuf(const uint8_t *rbuf) { {% for def_data in msg.def %} - {% if def_data.isArray%}{ - uint32_t arraySize; - memcpy(&arraySize,rbuf,4); - rbuf += 4; - {{def_data.typeName}}.reserve(arraySize); - for(int i=0;i0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; } + cntSub += 4-(cntSub%4); } - {% elif def_data.cppType == "string"%} uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; + cntSub += 4; {{def_data.typeName}}.resize(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); - rbuf += stringSize+1; + rbuf += stringSize; + cntSub += stringSize; + {% else %} + if (cntSub%4 >0 && {{def_data.size}} > 1){ + if (({{def_data.size}} <= (4-(cntSub%4)))&&({{def_data.size}}==2)){ + for (int i=0; i<(4-(cntSub%4))-{{def_data.size}}; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-{{def_data.size}}; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); - rbuf += 4; + rbuf += {{def_data.size}}; + cntSub += {{def_data.size}}; {% endif %} {% endfor %} } + uint8_t getTotalSize(){ + return cntPub ; + } private: std::string type_name = "{{msg.pkg}}::msg::dds_::{{msg.name}}"; From 319a7d0b523bbc330f454db88bbd3bbd021a942c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 19 Dec 2021 16:13:08 +0900 Subject: [PATCH 45/70] tpl filed modified --- .../msg_data_generator.cpython-37.pyc | Bin 0 -> 781 bytes .../msg_def_generator.cpython-37.pyc | Bin 0 -> 989 bytes mros2_header_generator/header_template.tpl | 109 ++++++---- mros2_msgs/flocation_msgs/msg/flocation.hpp | 194 ++++++++++++++++++ mros2_msgs/health_msgs/msg/health.hpp | 191 +++++++++++++++++ mros2_msgs/location_msgs/msg/location.hpp | 194 ++++++++++++++++++ 6 files changed, 643 insertions(+), 45 deletions(-) create mode 100644 mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc create mode 100644 mros2_header_generator/__pycache__/msg_def_generator.cpython-37.pyc create mode 100644 mros2_msgs/flocation_msgs/msg/flocation.hpp create mode 100644 mros2_msgs/health_msgs/msg/health.hpp create mode 100644 mros2_msgs/location_msgs/msg/location.hpp diff --git a/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..dee9a8ae87c41b16a853cf59b3ac8b036233b792 GIT binary patch literal 781 zcmYjPJ8u&~5T4oFJKMqGk%ADE!gA*#wgghrM2LW+6cG^+p`a7Ux@6b(5#NK|vy@op z2Gi3cio24AU%>xB+LkI3zaSN}c0!odd^@k5-I<+d%gakZ^lSfT@P`5Xw#qe;4o``7 zgFt}55%_jzBaE2O5QHQ6XYd_?htP5_C<+c*XzoOM(3Sl?8A}ypi8?{+A*vA9h@TVd z9|ReEz=GvaqWYRSIbAVhUpA+hW5G+9vH?t;vu5VzY=&KUfBzku@~N9Ug_l)wcZMY* zd~6@V5k2b_yoR@sLs2cEs0mi$)XP2LtU<9Pc!^fthJ}UDUAvNtTcwpqpG%JLzPX2x z)kP(TRS?xxnBgZBKqb}=45~})T-eS`c$?}}wzigQcpOC1IO*xY@PnNfyT0>sf6q8V z_9yo!tTB{_c%im2h9{|vO-<3UQ5Z{Y zYBC;2wx7~?K}t@c>?@3`GZm(^rK2!2p1M-U6rxOR_v}QbS(wDFis3eq??k=6?*vNu z-r}1Kh9vgp&WgnyrM`oDK*=wUJ8!g9x-$%q0~MSn;mI&mNvA9IX_lm&NG1BwMxfKj z8~v@#tx34BEU>pIxi`2TgAU!-6G0Z-nA+*Nxoe+6)_>#T(an0D0I*K0iX2yPo!!P2 z=Anm6w5pDJNd6XAc6xo=;U2a6Ch@#C^O$)d|G^p^FxBlS5#y13Vt-F_f-2VeU$E)X A@Bjb+ literal 0 HcmV?d00001 diff --git a/mros2_header_generator/__pycache__/msg_def_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/msg_def_generator.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e70d076e1ca1631109429526c1331a47682bf91f GIT binary patch literal 989 zcmZuvy>1gh5Z=9iJHGq?LJB{^719-nMG`qF5CRef38XX*%^_r+Gwb*i+k4tw7q*-V zc#9NJ@(y%71k$!teuWf-nL8{fxY2&|&3B`jxt)6%$90C!k2l}4?>=KcDOf(MNS@=- zuMvO&2M1j6z=r@rh@b{_)HtkM#VLcvAB!P|RkSrU!u6|0%3%X;ePpQvw>vDklLq4ylQt?5DJ2V$?Cvb3l9ANjTS~;N-yh1%qyflDZzyb_jmpR8No+mLJ+OY+ z!1`rT8&t*1aCfH)(bL=ZUKOGb%aW*9q`jvc{C?~8q#UMRZ)(JXqp%=#;Sod#Nux|t zF>>V+&?|*^%2YrQ5>Te95zw`(6@muB8C9f*@+tEWk2VN%_8D*f3%}%s&-n+=*&O$< z$u1rAE_&R^r8jqQE9?{M5L`Iw7{XFx;FE^Aj&UvmH6TAnW$qfc^d9arx(YhXaHvsr zV7Q_B$u(~-usc)q6C5vo-Q3*FoOka}lOPS1)Th%zq~Tdnlq513o@66oJ)O^l4RifM zscf1ya(y__Mvf?Lwn1slTsOzkH2ZQghQzbJE`~YovB13TX=Hvj$dzmz34LK?(Hh}* z>}+Ma*xl;yxA$jxrBv7*hzx}4o{O{% else %}{{def_data.cppType}}{% endif %} {{def_data.typeName}}; + {{def_data.cppType}} {{def_data.typeName}}; {% endfor %} - uint8_t getTotalSize(){ - return ({%for def_data in msg.def %} - {%if def_data.isArray %} - 4+{{def_data.size}}*({{def_data.typeName}}.size()) - + - {%elif def_data.cppType=="string"%} - 5+{{def_data.typeName}}.size() - + - {%else%} - 4 - + - {%endif%} - {%endfor%} - 0); - } - void copyToBuf(uint8_t *addrPtr) { {%for def_data in msg.def %} - - {% if def_data.isArray%}{ - uint32_t arraySize = {{def_data.typeName}}.size(); - memcpy(addrPtr,&arraySize,4); - addrPtr += 4; - const {{def_data.cppType}}* ptr = {{def_data.typeName}}.data(); - for(int i=0; i0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; } + cntPub += 4-(cntPub%4); } - - {% elif def_data.cppType == "string"%} uint32_t stringSize = {{def_data.typeName}}.size(); memcpy(addrPtr,&stringSize,4); addrPtr += 4; + cntPub += 4; memcpy(addrPtr,{{def_data.typeName}}.c_str(),stringSize); - addrPtr += stringSize+1; - *addrPtr = 0; + addrPtr += stringSize; + cntPub += stringSize; {% else %} + if (cntPub%4 >0 && {{def_data.size}} > 1){ + if (({{def_data.size}} <= (4-(cntPub%4)))&&({{def_data.size}}==2)){ + for (int i=0; i<(4-(cntPub%4))-{{def_data.size}}; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-{{def_data.size}}; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } memcpy(addrPtr,&{{def_data.typeName}},{{def_data.size}}); - addrPtr += 4; + addrPtr += {{def_data.size}}; + cntPub += {{def_data.size}}; {% endif %} - {% endfor %} + + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } } void copyFromBuf(const uint8_t *rbuf) { {% for def_data in msg.def %} - {% if def_data.isArray%}{ - uint32_t arraySize; - memcpy(&arraySize,rbuf,4); - rbuf += 4; - {{def_data.typeName}}.reserve(arraySize); - for(int i=0;i0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; } + cntSub += 4-(cntSub%4); } - {% elif def_data.cppType == "string"%} uint32_t stringSize; memcpy(&stringSize, rbuf, 4); rbuf += 4; + cntSub += 4; {{def_data.typeName}}.resize(stringSize); memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); - rbuf += stringSize+1; + rbuf += stringSize; + cntSub += stringSize; + {% else %} + if (cntSub%4 >0 && {{def_data.size}} > 1){ + if (({{def_data.size}} <= (4-(cntSub%4)))&&({{def_data.size}}==2)){ + for (int i=0; i<(4-(cntSub%4))-{{def_data.size}}; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-{{def_data.size}}; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); - rbuf += 4; + rbuf += {{def_data.size}}; + cntSub += {{def_data.size}}; {% endif %} {% endfor %} } + uint8_t getTotalSize(){ + return cntPub ; + } private: std::string type_name = "{{msg.pkg}}::msg::dds_::{{msg.name}}"; diff --git a/mros2_msgs/flocation_msgs/msg/flocation.hpp b/mros2_msgs/flocation_msgs/msg/flocation.hpp new file mode 100644 index 0000000..3935fb8 --- /dev/null +++ b/mros2_msgs/flocation_msgs/msg/flocation.hpp @@ -0,0 +1,194 @@ +#ifndef _FLOCATION_MSGS_MSG_FLOCATION_H +#define _FLOCATION_MSGS_MSG_FLOCATION_H + +#include +#include +#include + +using namespace std; + +namespace flocation_msgs +{ +namespace msg +{ +class Flocation +{ +public: + uint8_t cntPub = 0; + uint8_t cntSub = 0; + + float x +; + + float y +; + + float z; + + + void copyToBuf(uint8_t *addrPtr) + { + + + if (cntPub%4 >0 && 4 > 1){ + if ((4 <= (4-(cntPub%4)))&&(4==2)){ + for (int i=0; i<(4-(cntPub%4))-4; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-4; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&x +,4); + addrPtr += 4; + cntPub += 4; + + + + + if (cntPub%4 >0 && 4 > 1){ + if ((4 <= (4-(cntPub%4)))&&(4==2)){ + for (int i=0; i<(4-(cntPub%4))-4; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-4; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&y +,4); + addrPtr += 4; + cntPub += 4; + + + + + if (cntPub%4 >0 && 4 > 1){ + if ((4 <= (4-(cntPub%4)))&&(4==2)){ + for (int i=0; i<(4-(cntPub%4))-4; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-4; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&z,4); + addrPtr += 4; + cntPub += 4; + + + + + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + + void copyFromBuf(const uint8_t *rbuf) { + + + if (cntSub%4 >0 && 4 > 1){ + if ((4 <= (4-(cntSub%4)))&&(4==2)){ + for (int i=0; i<(4-(cntSub%4))-4; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-4; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&x +,rbuf,4); + rbuf += 4; + cntSub += 4; + + + + if (cntSub%4 >0 && 4 > 1){ + if ((4 <= (4-(cntSub%4)))&&(4==2)){ + for (int i=0; i<(4-(cntSub%4))-4; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-4; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&y +,rbuf,4); + rbuf += 4; + cntSub += 4; + + + + if (cntSub%4 >0 && 4 > 1){ + if ((4 <= (4-(cntSub%4)))&&(4==2)){ + for (int i=0; i<(4-(cntSub%4))-4; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-4; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&z,rbuf,4); + rbuf += 4; + cntSub += 4; + + + } + + uint8_t getTotalSize(){ + return cntPub ; + } + +private: + std::string type_name = "flocation_msgs::msg::dds_::Flocation"; +}; +}; +} + +namespace message_traits +{ +template<> +struct TypeName { + static const char* value() + { + return "flocation_msgs::msg::dds_::Flocation_"; + } +}; +} + +#endif \ No newline at end of file diff --git a/mros2_msgs/health_msgs/msg/health.hpp b/mros2_msgs/health_msgs/msg/health.hpp new file mode 100644 index 0000000..868631e --- /dev/null +++ b/mros2_msgs/health_msgs/msg/health.hpp @@ -0,0 +1,191 @@ +#ifndef _HEALTH_MSGS_MSG_HEALTH_H +#define _HEALTH_MSGS_MSG_HEALTH_H + +#include +#include +#include + +using namespace std; + +namespace health_msgs +{ +namespace msg +{ +class Health +{ +public: + uint8_t cntPub = 0; + uint8_t cntSub = 0; + + string name +; + + uint16_t height +; + + float weight; + + + void copyToBuf(uint8_t *addrPtr) + { + + + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + uint32_t stringSize = name +.size(); + memcpy(addrPtr,&stringSize,4); + addrPtr += 4; + cntPub += 4; + memcpy(addrPtr,name +.c_str(),stringSize); + addrPtr += stringSize; + cntPub += stringSize; + + + + + if (cntPub%4 >0 && 2 > 1){ + if ((2 <= (4-(cntPub%4)))&&(2==2)){ + for (int i=0; i<(4-(cntPub%4))-2; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-2; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&height +,2); + addrPtr += 2; + cntPub += 2; + + + + + if (cntPub%4 >0 && 4 > 1){ + if ((4 <= (4-(cntPub%4)))&&(4==2)){ + for (int i=0; i<(4-(cntPub%4))-4; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-4; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&weight,4); + addrPtr += 4; + cntPub += 4; + + + + + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + + void copyFromBuf(const uint8_t *rbuf) { + + + if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + uint32_t stringSize; + memcpy(&stringSize, rbuf, 4); + rbuf += 4; + cntSub += 4; + name +.resize(stringSize); + memcpy(&name +[0],rbuf,stringSize); + rbuf += stringSize; + cntSub += stringSize; + + + + + if (cntSub%4 >0 && 2 > 1){ + if ((2 <= (4-(cntSub%4)))&&(2==2)){ + for (int i=0; i<(4-(cntSub%4))-2; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-2; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&height +,rbuf,2); + rbuf += 2; + cntSub += 2; + + + + if (cntSub%4 >0 && 4 > 1){ + if ((4 <= (4-(cntSub%4)))&&(4==2)){ + for (int i=0; i<(4-(cntSub%4))-4; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-4; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&weight,rbuf,4); + rbuf += 4; + cntSub += 4; + + + } + + uint8_t getTotalSize(){ + return cntPub ; + } + +private: + std::string type_name = "health_msgs::msg::dds_::Health"; +}; +}; +} + +namespace message_traits +{ +template<> +struct TypeName { + static const char* value() + { + return "health_msgs::msg::dds_::Health_"; + } +}; +} + +#endif \ No newline at end of file diff --git a/mros2_msgs/location_msgs/msg/location.hpp b/mros2_msgs/location_msgs/msg/location.hpp new file mode 100644 index 0000000..73f90ec --- /dev/null +++ b/mros2_msgs/location_msgs/msg/location.hpp @@ -0,0 +1,194 @@ +#ifndef _LOCATION_MSGS_MSG_LOCATION_H +#define _LOCATION_MSGS_MSG_LOCATION_H + +#include +#include +#include + +using namespace std; + +namespace location_msgs +{ +namespace msg +{ +class Location +{ +public: + uint8_t cntPub = 0; + uint8_t cntSub = 0; + + int16_t x +; + + int16_t y +; + + int16_t z; + + + void copyToBuf(uint8_t *addrPtr) + { + + + if (cntPub%4 >0 && 2 > 1){ + if ((2 <= (4-(cntPub%4)))&&(2==2)){ + for (int i=0; i<(4-(cntPub%4))-2; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-2; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&x +,2); + addrPtr += 2; + cntPub += 2; + + + + + if (cntPub%4 >0 && 2 > 1){ + if ((2 <= (4-(cntPub%4)))&&(2==2)){ + for (int i=0; i<(4-(cntPub%4))-2; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-2; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&y +,2); + addrPtr += 2; + cntPub += 2; + + + + + if (cntPub%4 >0 && 2 > 1){ + if ((2 <= (4-(cntPub%4)))&&(2==2)){ + for (int i=0; i<(4-(cntPub%4))-2; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-2; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&z,2); + addrPtr += 2; + cntPub += 2; + + + + + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + + void copyFromBuf(const uint8_t *rbuf) { + + + if (cntSub%4 >0 && 2 > 1){ + if ((2 <= (4-(cntSub%4)))&&(2==2)){ + for (int i=0; i<(4-(cntSub%4))-2; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-2; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&x +,rbuf,2); + rbuf += 2; + cntSub += 2; + + + + if (cntSub%4 >0 && 2 > 1){ + if ((2 <= (4-(cntSub%4)))&&(2==2)){ + for (int i=0; i<(4-(cntSub%4))-2; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-2; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&y +,rbuf,2); + rbuf += 2; + cntSub += 2; + + + + if (cntSub%4 >0 && 2 > 1){ + if ((2 <= (4-(cntSub%4)))&&(2==2)){ + for (int i=0; i<(4-(cntSub%4))-2; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-2; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&z,rbuf,2); + rbuf += 2; + cntSub += 2; + + + } + + uint8_t getTotalSize(){ + return cntPub ; + } + +private: + std::string type_name = "location_msgs::msg::dds_::Location"; +}; +}; +} + +namespace message_traits +{ +template<> +struct TypeName { + static const char* value() + { + return "location_msgs::msg::dds_::Location_"; + } +}; +} + +#endif \ No newline at end of file From 75e37b762c64306abfee0e3488c30666a781ed74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Wed, 22 Dec 2021 12:14:55 +0900 Subject: [PATCH 46/70] tpl modified for support array --- mros2_header_generator/header_generator.py | 6 ++- mros2_header_generator/header_template.tpl | 46 ++++++++++++++++++++-- src/mros2.cpp | 23 +++++++---- 3 files changed, 63 insertions(+), 12 deletions(-) diff --git a/mros2_header_generator/header_generator.py b/mros2_header_generator/header_generator.py index 55bfa16..fc792c4 100644 --- a/mros2_header_generator/header_generator.py +++ b/mros2_header_generator/header_generator.py @@ -3,6 +3,7 @@ import os import json import sys +import re from jinja2 import Environment, FileSystemLoader from msg_data_generator import msgDataGenerator @@ -36,6 +37,9 @@ msgIncludePath = appDir + "/" + "mros2_msgs" + "/" fileDir = os.getcwd() +def toSnakeCase(string): + return re.sub("(.[A-Z])",lambda x:x.group(1)[0] + "_" +x.group(1)[1],string).lower() + def main(): #if not(os.path.isdir(msgIncludePath)): #os.mkdir(msgIncludePath) @@ -62,7 +66,7 @@ def main(): if not(os.path.isdir(msgPkgPath)): os.mkdir(msgPkgPath) os.mkdir(msgPkgPath + "/msg") - with open(os.path.join(msgPkgPath, "msg", msg['name'].lower() + ".hpp"), "wb") as f: + with open(os.path.join(msgPkgPath, "msg", toSnakeCase(msg['name']) + ".hpp"), "wb") as f: f.write(datatext.encode('utf-8')) if __name__ == "__main__": diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 84d1229..5d81048 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -17,13 +17,33 @@ public: uint8_t cntPub = 0; uint8_t cntSub = 0; {%for def_data in msg.def %} - {{def_data.cppType}} {{def_data.typeName}}; + {%if def_data.isArray %}std::vector<{{def_data.cppType}}>{% else %}{{def_data.cppType}}{% endif %} {{def_data.typeName}}; {% endfor %} void copyToBuf(uint8_t *addrPtr) { {%for def_data in msg.def %} - {% if def_data.cppType == "string"%} + {% if def_data.isArray%}{ + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + uint32_t arraySize = {{def_data.typeName}}.size(); + memcpy(addrPtr,&arraySize,4); + addrPtr += 4; + cntPub += 4; + const {{def_data.cppType}}* ptr = {{def_data.typeName}}.data(); + for(int i=0; i0){ for(int i=0; i<(4-(cntPub%4)) ; i++){ *addrPtr = 0; @@ -73,7 +93,27 @@ public: void copyFromBuf(const uint8_t *rbuf) { {% for def_data in msg.def %} - {% if def_data.cppType == "string"%} + {% if def_data.isArray%}{ + if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + uint32_t arraySize; + memcpy(&arraySize,rbuf,4); + rbuf += 4; + cntSub += 4; + {{def_data.typeName}}.reserve(arraySize); + for(int i=0;i0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ rbuf += 1; diff --git a/src/mros2.cpp b/src/mros2.cpp index 6bae14e..e3882b9 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -24,15 +24,17 @@ #include "std_msgs/msg/u_int32.hpp" #include "std_msgs/msg/u_int64.hpp" +#include "u_int32_msgs/msg/u_int32_array.hpp" + //#include "TEST.hpp" #include "health_msgs/msg/health.hpp" #include "location_msgs/msg/location.hpp" -#include "flocation_msgs/msg/flocation.hpp" +#include "float_location_msgs/msg/float_location.hpp" -#ifdef USE_ASP3_FOR_STM +#ifndef __MBED__ /* Statement to avoid link error */ void* __dso_handle=0; -#endif /* USE_ASP3_FOR_STM */ +#endif /* __MBED__ */ namespace mros2 @@ -241,7 +243,7 @@ template void Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange) { T msg; - msg.copyFromBuf(&cacheChange.getData()[4]); + msg.copyFromBuf(&cacheChange.data[4]); SubscribeDataType *sub = (SubscribeDataType *)callee; void (*fp)(intptr_t) = sub->cb_fp; @@ -389,7 +391,12 @@ template mros2::Subscriber mros2::Node::create_subscription(std::string topic_na template void mros2::Publisher::publish(location_msgs::msg::Location &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(flocation_msgs::msg::Flocation*)); -template void mros2::Publisher::publish(flocation_msgs::msg::Flocation &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(float_location_msgs::msg::FloatLocation*)); +template void mros2::Publisher::publish(float_location_msgs::msg::FloatLocation &msg); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); + +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::UInt32Array*)); +template void mros2::Publisher::publish(std_msgs::msg::UInt32Array &msg); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); From 5dfd3b6348746f755e87b840555f851f8657cb5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Wed, 22 Dec 2021 12:24:17 +0900 Subject: [PATCH 47/70] tpl modified for support array --- .../u_int32_array_msgs/msg/u_int32_array.hpp | 105 ++++++++++++++++++ src/mros2.cpp | 17 ++- 2 files changed, 113 insertions(+), 9 deletions(-) create mode 100644 mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp diff --git a/mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp b/mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp new file mode 100644 index 0000000..40627be --- /dev/null +++ b/mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp @@ -0,0 +1,105 @@ +#ifndef _U_INT32_ARRAY_MSGS_MSG_UINT32ARRAY_H +#define _U_INT32_ARRAY_MSGS_MSG_UINT32ARRAY_H + +#include +#include +#include + +using namespace std; + +namespace u_int32_array_msgs +{ +namespace msg +{ +class UInt32Array +{ +public: + uint8_t cntPub = 0; + uint8_t cntSub = 0; + + std::vector data; + + + void copyToBuf(uint8_t *addrPtr) + { + + { + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + uint32_t arraySize = data.size(); + memcpy(addrPtr,&arraySize,4); + addrPtr += 4; + cntPub += 4; + const uint32_t* ptr = data.data(); + for(int i=0; i0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + + void copyFromBuf(const uint8_t *rbuf) { + + { + if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + uint32_t arraySize; + memcpy(&arraySize,rbuf,4); + rbuf += 4; + cntSub += 4; + data.reserve(arraySize); + for(int i=0;i +struct TypeName { + static const char* value() + { + return "u_int32_array_msgs::msg::dds_::UInt32Array_"; + } +}; +} + +#endif \ No newline at end of file diff --git a/src/mros2.cpp b/src/mros2.cpp index e3882b9..1e6238b 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -24,12 +24,11 @@ #include "std_msgs/msg/u_int32.hpp" #include "std_msgs/msg/u_int64.hpp" -#include "u_int32_msgs/msg/u_int32_array.hpp" - //#include "TEST.hpp" #include "health_msgs/msg/health.hpp" #include "location_msgs/msg/location.hpp" -#include "float_location_msgs/msg/float_location.hpp" +//#include "float_location_msgs/msg/float_location.hpp" +#include "u_int32_array_msgs/msg/u_int32_array.hpp" #ifndef __MBED__ /* Statement to avoid link error */ @@ -391,12 +390,12 @@ template mros2::Subscriber mros2::Node::create_subscription(std::string topic_na template void mros2::Publisher::publish(location_msgs::msg::Location &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +/*template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(float_location_msgs::msg::FloatLocation*)); template void mros2::Publisher::publish(float_location_msgs::msg::FloatLocation &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange);*/ -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::UInt32Array*)); -template void mros2::Publisher::publish(std_msgs::msg::UInt32Array &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::u_int32_array_msgs::msg::UInt32Array*)); +template void mros2::Publisher::publish(u_int32_array_msgs::msg::UInt32Array &msg); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); From 60b810d85d4fca45f4580746e23d8562942b64c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Wed, 22 Dec 2021 12:26:00 +0900 Subject: [PATCH 48/70] tpl modified for support array --- src/mros2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mros2.cpp b/src/mros2.cpp index 1e6238b..3de749a 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -396,6 +396,6 @@ template void mros2::Publisher::publish(float_location_msgs::msg::FloatLocation template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange);*/ template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::u_int32_array_msgs::msg::UInt32Array*)); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(u_int32_array_msgs::msg::UInt32Array*)); template void mros2::Publisher::publish(u_int32_array_msgs::msg::UInt32Array &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); From 78dbbe66e2bdf8c9364c264c8ebe47f553d36d5c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Wed, 22 Dec 2021 12:44:31 +0900 Subject: [PATCH 49/70] tpl modified for support array --- mros2_header_generator/header_template.tpl | 7 +++---- mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp | 7 +++---- src/mros2.cpp | 10 +++++----- 3 files changed, 11 insertions(+), 13 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 5d81048..e3deaf6 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -23,7 +23,7 @@ public: void copyToBuf(uint8_t *addrPtr) { {%for def_data in msg.def %} - {% if def_data.isArray%}{ + {% if def_data.isArray%} if (cntPub%4 >0){ for(int i=0; i<(4-(cntPub%4)) ; i++){ *addrPtr = 0; @@ -41,8 +41,7 @@ public: addrPtr += {{def_data.size}}; cntPub += {{def_data.size}}; } - } - + {% elif def_data.cppType == "string"%} if (cntPub%4 >0){ for(int i=0; i<(4-(cntPub%4)) ; i++){ @@ -93,7 +92,7 @@ public: void copyFromBuf(const uint8_t *rbuf) { {% for def_data in msg.def %} - {% if def_data.isArray%}{ + {% if def_data.isArray%} if (cntSub%4 >0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ rbuf += 1; diff --git a/mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp b/mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp index 40627be..5eb3db0 100644 --- a/mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp +++ b/mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp @@ -23,7 +23,7 @@ class UInt32Array void copyToBuf(uint8_t *addrPtr) { - { + if (cntPub%4 >0){ for(int i=0; i<(4-(cntPub%4)) ; i++){ *addrPtr = 0; @@ -41,8 +41,7 @@ class UInt32Array addrPtr += 4; cntPub += 4; } - } - + @@ -57,7 +56,7 @@ class UInt32Array void copyFromBuf(const uint8_t *rbuf) { - { + if (cntSub%4 >0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ rbuf += 1; diff --git a/src/mros2.cpp b/src/mros2.cpp index 3de749a..e4f7e09 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -390,12 +390,12 @@ template mros2::Subscriber mros2::Node::create_subscription(std::string topic_na template void mros2::Publisher::publish(location_msgs::msg::Location &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); -/*template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(float_location_msgs::msg::FloatLocation*)); -template void mros2::Publisher::publish(float_location_msgs::msg::FloatLocation &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange);*/ - template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(u_int32_array_msgs::msg::UInt32Array*)); template void mros2::Publisher::publish(u_int32_array_msgs::msg::UInt32Array &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); + +/*template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(float_location_msgs::msg::FloatLocation*)); +template void mros2::Publisher::publish(float_location_msgs::msg::FloatLocation &msg); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange);*/ From 53100c883a7e34f76527959f95f7e150ac63d333 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Wed, 22 Dec 2021 16:40:51 +0900 Subject: [PATCH 50/70] tpl modified for support array --- mros2_msgs/flocation_msgs/msg/flocation.hpp | 194 -------------- mros2_msgs/mix_msgs/msg/mix.hpp | 239 ++++++++++++++++++ .../u_int32_array_msgs/msg/u_int32_array.hpp | 104 -------- src/mros2.cpp | 15 +- 4 files changed, 244 insertions(+), 308 deletions(-) delete mode 100644 mros2_msgs/flocation_msgs/msg/flocation.hpp create mode 100644 mros2_msgs/mix_msgs/msg/mix.hpp delete mode 100644 mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp diff --git a/mros2_msgs/flocation_msgs/msg/flocation.hpp b/mros2_msgs/flocation_msgs/msg/flocation.hpp deleted file mode 100644 index 3935fb8..0000000 --- a/mros2_msgs/flocation_msgs/msg/flocation.hpp +++ /dev/null @@ -1,194 +0,0 @@ -#ifndef _FLOCATION_MSGS_MSG_FLOCATION_H -#define _FLOCATION_MSGS_MSG_FLOCATION_H - -#include -#include -#include - -using namespace std; - -namespace flocation_msgs -{ -namespace msg -{ -class Flocation -{ -public: - uint8_t cntPub = 0; - uint8_t cntSub = 0; - - float x -; - - float y -; - - float z; - - - void copyToBuf(uint8_t *addrPtr) - { - - - if (cntPub%4 >0 && 4 > 1){ - if ((4 <= (4-(cntPub%4)))&&(4==2)){ - for (int i=0; i<(4-(cntPub%4))-4; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-4; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&x -,4); - addrPtr += 4; - cntPub += 4; - - - - - if (cntPub%4 >0 && 4 > 1){ - if ((4 <= (4-(cntPub%4)))&&(4==2)){ - for (int i=0; i<(4-(cntPub%4))-4; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-4; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&y -,4); - addrPtr += 4; - cntPub += 4; - - - - - if (cntPub%4 >0 && 4 > 1){ - if ((4 <= (4-(cntPub%4)))&&(4==2)){ - for (int i=0; i<(4-(cntPub%4))-4; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-4; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&z,4); - addrPtr += 4; - cntPub += 4; - - - - - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - - void copyFromBuf(const uint8_t *rbuf) { - - - if (cntSub%4 >0 && 4 > 1){ - if ((4 <= (4-(cntSub%4)))&&(4==2)){ - for (int i=0; i<(4-(cntSub%4))-4; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-4; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&x -,rbuf,4); - rbuf += 4; - cntSub += 4; - - - - if (cntSub%4 >0 && 4 > 1){ - if ((4 <= (4-(cntSub%4)))&&(4==2)){ - for (int i=0; i<(4-(cntSub%4))-4; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-4; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&y -,rbuf,4); - rbuf += 4; - cntSub += 4; - - - - if (cntSub%4 >0 && 4 > 1){ - if ((4 <= (4-(cntSub%4)))&&(4==2)){ - for (int i=0; i<(4-(cntSub%4))-4; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-4; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&z,rbuf,4); - rbuf += 4; - cntSub += 4; - - - } - - uint8_t getTotalSize(){ - return cntPub ; - } - -private: - std::string type_name = "flocation_msgs::msg::dds_::Flocation"; -}; -}; -} - -namespace message_traits -{ -template<> -struct TypeName { - static const char* value() - { - return "flocation_msgs::msg::dds_::Flocation_"; - } -}; -} - -#endif \ No newline at end of file diff --git a/mros2_msgs/mix_msgs/msg/mix.hpp b/mros2_msgs/mix_msgs/msg/mix.hpp new file mode 100644 index 0000000..b601e56 --- /dev/null +++ b/mros2_msgs/mix_msgs/msg/mix.hpp @@ -0,0 +1,239 @@ +#ifndef _MIX_MSGS_MSG_MIX_H +#define _MIX_MSGS_MSG_MIX_H + +#include +#include +#include + +using namespace std; + +namespace mix_msgs +{ +namespace msg +{ +class Mix +{ +public: + uint8_t cntPub = 0; + uint8_t cntSub = 0; + + string name +; + + uint16_t height +; + + float weight +; + + std::vector array; + + + void copyToBuf(uint8_t *addrPtr) + { + + + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + uint32_t stringSize = name +.size(); + memcpy(addrPtr,&stringSize,4); + addrPtr += 4; + cntPub += 4; + memcpy(addrPtr,name +.c_str(),stringSize); + addrPtr += stringSize; + cntPub += stringSize; + + + + + if (cntPub%4 >0 && 2 > 1){ + if ((2 <= (4-(cntPub%4)))&&(2==2)){ + for (int i=0; i<(4-(cntPub%4))-2; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-2; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&height +,2); + addrPtr += 2; + cntPub += 2; + + + + + if (cntPub%4 >0 && 4 > 1){ + if ((4 <= (4-(cntPub%4)))&&(4==2)){ + for (int i=0; i<(4-(cntPub%4))-4; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-4; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&weight +,4); + addrPtr += 4; + cntPub += 4; + + + + + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + uint32_t arraySize = array.size(); + memcpy(addrPtr,&arraySize,4); + addrPtr += 4; + cntPub += 4; + const uint32_t* ptr = array.data(); + for(int i=0; i0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + + void copyFromBuf(const uint8_t *rbuf) { + + + if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + uint32_t stringSize; + memcpy(&stringSize, rbuf, 4); + rbuf += 4; + cntSub += 4; + name +.resize(stringSize); + memcpy(&name +[0],rbuf,stringSize); + rbuf += stringSize; + cntSub += stringSize; + + + + + if (cntSub%4 >0 && 2 > 1){ + if ((2 <= (4-(cntSub%4)))&&(2==2)){ + for (int i=0; i<(4-(cntSub%4))-2; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-2; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&height +,rbuf,2); + rbuf += 2; + cntSub += 2; + + + + if (cntSub%4 >0 && 4 > 1){ + if ((4 <= (4-(cntSub%4)))&&(4==2)){ + for (int i=0; i<(4-(cntSub%4))-4; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-4; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&weight +,rbuf,4); + rbuf += 4; + cntSub += 4; + + + + if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + uint32_t arraySize; + memcpy(&arraySize,rbuf,4); + rbuf += 4; + cntSub += 4; + array.reserve(arraySize); + for(int i=0;i +struct TypeName { + static const char* value() + { + return "mix_msgs::msg::dds_::Mix_"; + } +}; +} + +#endif \ No newline at end of file diff --git a/mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp b/mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp deleted file mode 100644 index 5eb3db0..0000000 --- a/mros2_msgs/u_int32_array_msgs/msg/u_int32_array.hpp +++ /dev/null @@ -1,104 +0,0 @@ -#ifndef _U_INT32_ARRAY_MSGS_MSG_UINT32ARRAY_H -#define _U_INT32_ARRAY_MSGS_MSG_UINT32ARRAY_H - -#include -#include -#include - -using namespace std; - -namespace u_int32_array_msgs -{ -namespace msg -{ -class UInt32Array -{ -public: - uint8_t cntPub = 0; - uint8_t cntSub = 0; - - std::vector data; - - - void copyToBuf(uint8_t *addrPtr) - { - - - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - uint32_t arraySize = data.size(); - memcpy(addrPtr,&arraySize,4); - addrPtr += 4; - cntPub += 4; - const uint32_t* ptr = data.data(); - for(int i=0; i0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - - void copyFromBuf(const uint8_t *rbuf) { - - - if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - uint32_t arraySize; - memcpy(&arraySize,rbuf,4); - rbuf += 4; - cntSub += 4; - data.reserve(arraySize); - for(int i=0;i -struct TypeName { - static const char* value() - { - return "u_int32_array_msgs::msg::dds_::UInt32Array_"; - } -}; -} - -#endif \ No newline at end of file diff --git a/src/mros2.cpp b/src/mros2.cpp index e4f7e09..3a793da 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -27,8 +27,7 @@ //#include "TEST.hpp" #include "health_msgs/msg/health.hpp" #include "location_msgs/msg/location.hpp" -//#include "float_location_msgs/msg/float_location.hpp" -#include "u_int32_array_msgs/msg/u_int32_array.hpp" +#include "mix_msgs/msg/mix.hpp" #ifndef __MBED__ /* Statement to avoid link error */ @@ -390,12 +389,8 @@ template mros2::Subscriber mros2::Node::create_subscription(std::string topic_na template void mros2::Publisher::publish(location_msgs::msg::Location &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(u_int32_array_msgs::msg::UInt32Array*)); -template void mros2::Publisher::publish(u_int32_array_msgs::msg::UInt32Array &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(mix_msgs::msg::Mix*)); +template void mros2::Publisher::publish(mix_msgs::msg::Mix &msg); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); -/*template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(float_location_msgs::msg::FloatLocation*)); -template void mros2::Publisher::publish(float_location_msgs::msg::FloatLocation &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange);*/ From b8a616f545ec147bfa32b47f6e696665528ddf82 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 23 Dec 2021 00:20:14 +0900 Subject: [PATCH 51/70] vector3 app added --- mros2_msgs/geometry_msgs/msg/vector3.hpp | 194 +++++++++++++++++++++++ mros2_msgs/mix_msgs/msg/mix.hpp | 18 +-- src/mros2.cpp | 6 + 3 files changed, 209 insertions(+), 9 deletions(-) create mode 100644 mros2_msgs/geometry_msgs/msg/vector3.hpp diff --git a/mros2_msgs/geometry_msgs/msg/vector3.hpp b/mros2_msgs/geometry_msgs/msg/vector3.hpp new file mode 100644 index 0000000..f285ed6 --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/vector3.hpp @@ -0,0 +1,194 @@ +#ifndef _GEOMETRY_MSGS_MSG_VECTOR3_H +#define _GEOMETRY_MSGS_MSG_VECTOR3_H + +#include +#include +#include + +using namespace std; + +namespace geometry_msgs +{ +namespace msg +{ +class Vector3 +{ +public: + uint8_t cntPub = 0; + uint8_t cntSub = 0; + + double x +; + + double y +; + + double z; + + + void copyToBuf(uint8_t *addrPtr) + { + + + if (cntPub%4 >0 && 8 > 1){ + if ((8 <= (4-(cntPub%4)))&&(8==2)){ + for (int i=0; i<(4-(cntPub%4))-8; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-8; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&x +,8); + addrPtr += 8; + cntPub += 8; + + + + + if (cntPub%4 >0 && 8 > 1){ + if ((8 <= (4-(cntPub%4)))&&(8==2)){ + for (int i=0; i<(4-(cntPub%4))-8; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-8; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&y +,8); + addrPtr += 8; + cntPub += 8; + + + + + if (cntPub%4 >0 && 8 > 1){ + if ((8 <= (4-(cntPub%4)))&&(8==2)){ + for (int i=0; i<(4-(cntPub%4))-8; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += (4-(cntPub%4))-8; + } else { + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + memcpy(addrPtr,&z,8); + addrPtr += 8; + cntPub += 8; + + + + + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } + + void copyFromBuf(const uint8_t *rbuf) { + + + if (cntSub%4 >0 && 8 > 1){ + if ((8 <= (4-(cntSub%4)))&&(8==2)){ + for (int i=0; i<(4-(cntSub%4))-8; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-8; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&x +,rbuf,8); + rbuf += 8; + cntSub += 8; + + + + if (cntSub%4 >0 && 8 > 1){ + if ((8 <= (4-(cntSub%4)))&&(8==2)){ + for (int i=0; i<(4-(cntSub%4))-8; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-8; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&y +,rbuf,8); + rbuf += 8; + cntSub += 8; + + + + if (cntSub%4 >0 && 8 > 1){ + if ((8 <= (4-(cntSub%4)))&&(8==2)){ + for (int i=0; i<(4-(cntSub%4))-8; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-8; + } else { + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } + memcpy(&z,rbuf,8); + rbuf += 8; + cntSub += 8; + + + } + + uint8_t getTotalSize(){ + return cntPub ; + } + +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 diff --git a/mros2_msgs/mix_msgs/msg/mix.hpp b/mros2_msgs/mix_msgs/msg/mix.hpp index b601e56..2d305bc 100644 --- a/mros2_msgs/mix_msgs/msg/mix.hpp +++ b/mros2_msgs/mix_msgs/msg/mix.hpp @@ -26,7 +26,7 @@ class Mix float weight ; - std::vector array; + std::vector array; void copyToBuf(uint8_t *addrPtr) @@ -110,11 +110,11 @@ class Mix memcpy(addrPtr,&arraySize,4); addrPtr += 4; cntPub += 4; - const uint32_t* ptr = array.data(); + const uint16_t* ptr = array.data(); for(int i=0; i(void *callee, const rtps::ReaderCacheChange &cacheChange); +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(geometry_msgs::msg::Vector3*)); +template void mros2::Publisher::publish(geometry_msgs::msg::Vector3 &msg); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); + template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(mix_msgs::msg::Mix*)); template void mros2::Publisher::publish(mix_msgs::msg::Mix &msg); From 24439e42a3911fa32a5ddaaae1bdc1df1537401b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 23 Dec 2021 11:29:04 +0900 Subject: [PATCH 52/70] mros2_header_generator modified for support nested custom type --- mros2_header_generator/header_generator.py | 41 +++++++------------- mros2_header_generator/header_template.tpl | 22 +++++++++-- mros2_header_generator/msg_data_generator.py | 6 +-- mros2_header_generator/msg_def_generator.py | 14 ++++++- 4 files changed, 48 insertions(+), 35 deletions(-) diff --git a/mros2_header_generator/header_generator.py b/mros2_header_generator/header_generator.py index fc792c4..1cef386 100644 --- a/mros2_header_generator/header_generator.py +++ b/mros2_header_generator/header_generator.py @@ -7,33 +7,16 @@ from jinja2 import Environment, FileSystemLoader from msg_data_generator import msgDataGenerator -# standard types for ROS2 -stdMsgs = { - "std_msgs/msg/int8.hpp": 1, - "std_msgs/msg/u_int8.hpp": 2, - "std_msgs/msg/int16.hpp": 3, - "std_msgs/msg/u_int16.hpp": 4, - "std_msgs/msg/int32.hpp": 5, - "std_msgs/msg/u_int32.hpp": 6, - "std_msgs/msg/int64.hpp": 7, - "std_msgs/msg/u_int64.hpp": 8, - "std_msgs/msg/float32.hpp": 9, - "std_msgs/msg/float64.hpp": 10, - "std_msgs/msg/string.hpp": 11, - "std_msgs/msg/bool.hpp": 12, - "std_msgs/msg/byte.hpp": 13, -} - msgs = [] -depMsgs = [] -includedStdMsgs = [] +deps = [] +dependingMsgs = [] arg = sys.argv if(len(arg) == 1): raise Exception('Error: json file is not specified') jsonFileName = arg[1] -appDir = '../../mros2' +appDir = '../../mros2' # for asp_app msgIncludePath = appDir + "/" + "mros2_msgs" + "/" fileDir = os.getcwd() @@ -48,19 +31,25 @@ def main(): with open(fileDir + "/" + jsonFileName, 'r') as f: jsonData = json.load(f) + + for dep in jsonData['dependingMsgs']: + dep = toSnakeCase(dep.strip()[:-4].replace('/','::')) + '.hpp' + deps.append(dep) + + for dep in jsonData['dependingMsgs']: + depArr = dep.strip().split('/') + depArr[2] = depArr[2].rstrip('.msg') + dependingMsgs.append(depArr[2]) + for line in jsonData['includingMsgs']: line = line.strip() - print(line) - if line not in stdMsgs: # when custom type - msgs.append(msgDataGenerator(line)) # generate message data of the custom type - - #os.chdir(fileDir) + msgs.append(msgDataGenerator(line, dependingMsgs)) # generate message data of the custom type # generate header file for mros2 for msg in msgs: env = Environment(loader=FileSystemLoader(appDir + '/mros2_header_generator')) template = env.get_template('header_template.tpl') - datatext = template.render({"msg": msg}) + datatext = template.render({"msg": msg, "deps": deps}) print(datatext) msgPkgPath = msgIncludePath + msg['pkg'] if not(os.path.isdir(msgPkgPath)): diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index e3deaf6..6f6f1de 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -5,6 +5,10 @@ #include #include +{%for dep in deps%} +#include {{dep}} +{%endfor} + using namespace std; namespace {{msg.pkg}} @@ -20,10 +24,13 @@ public: {%if def_data.isArray %}std::vector<{{def_data.cppType}}>{% else %}{{def_data.cppType}}{% endif %} {{def_data.typeName}}; {% endfor %} - void copyToBuf(uint8_t *addrPtr) + uint8_t copyToBuf(uint8_t *addrPtr) { {%for def_data in msg.def %} - {% if def_data.isArray%} + {% if def_data.isCustom%} + cntPub += {{def_data.typeName}}.copyToBuf(addrPtr); + + {% elif def_data.isArray%} if (cntPub%4 >0){ for(int i=0; i<(4-(cntPub%4)) ; i++){ *addrPtr = 0; @@ -88,11 +95,16 @@ public: } cntPub += 4-(cntPub%4); } + + return cntPub; } - void copyFromBuf(const uint8_t *rbuf) { + uint8_t copyFromBuf(const uint8_t *rbuf) { {% for def_data in msg.def %} - {% if def_data.isArray%} + {% if def_data.isCustom%} + cntSub += {{def_data.typeName}}.copyFromBuf(rbuf); + + {% elif def_data.isArray%} if (cntSub%4 >0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ rbuf += 1; @@ -147,6 +159,8 @@ public: cntSub += {{def_data.size}}; {% endif %} {% endfor %} + + return cntSub; } uint8_t getTotalSize(){ diff --git a/mros2_header_generator/msg_data_generator.py b/mros2_header_generator/msg_data_generator.py index 57716b9..d2ed4e4 100644 --- a/mros2_header_generator/msg_data_generator.py +++ b/mros2_header_generator/msg_data_generator.py @@ -3,13 +3,13 @@ # generate message data is mainly about the name, definition of the type -def msgDataGenerator(line): +def msgDataGenerator(line, dependingMsgs): if os.path.isfile(line): with open(line, 'r') as m_f: arr = m_f.readlines() msgDef = [] - for i, m_line in enumerate(arr): - msgDef.append(msgDefGenerator(m_line)) + for m_line in arr: + msgDef.append(msgDefGenerator(m_line, dependingMsgs)) lineArr = line.strip().split('/') lineArr[2] = lineArr[2].rstrip('.msg') diff --git a/mros2_header_generator/msg_def_generator.py b/mros2_header_generator/msg_def_generator.py index 4c181ec..4b10c3f 100644 --- a/mros2_header_generator/msg_def_generator.py +++ b/mros2_header_generator/msg_def_generator.py @@ -28,7 +28,7 @@ # generate detail data of type definition -def msgDefGenerator(msgDefStr): +def msgDefGenerator(msgDefStr, dependingMsgs): # split the type def and name of the type ('string name' -> ['string', 'name']) msgDefArr = msgDefStr.split(' ') @@ -51,6 +51,16 @@ def msgDefGenerator(msgDefStr): 'isArray': isArray, 'isCustomType': False } - + + elif msgType in dependingMsgs: + return { + 'rosType': msgType, + 'cppType': msgType, + 'typeName': msgName, + 'size': msgSizes[msgType], + 'isArray': isArray, + 'isCustomType': True + } + else: print('type is not found') \ No newline at end of file From 8ceaefacb66101b0ae751ad3bb6761cddcbb9e79 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 23 Dec 2021 12:02:22 +0900 Subject: [PATCH 53/70] mros2_header_generator modified for support nested custom type --- .../msg_data_generator.cpython-37.pyc | Bin 781 -> 776 bytes .../msg_def_generator.cpython-37.pyc | Bin 989 -> 1052 bytes mros2_header_generator/header_generator.py | 13 ++- mros2_header_generator/header_template.tpl | 10 +- mros2_header_generator/msg_def_generator.py | 6 +- mros2_msgs/geometry_msgs/msg/twist.hpp | 94 ++++++++++++++++++ 6 files changed, 110 insertions(+), 13 deletions(-) create mode 100644 mros2_msgs/geometry_msgs/msg/twist.hpp diff --git a/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc index dee9a8ae87c41b16a853cf59b3ac8b036233b792..e213092706801be5de645505bf2f6c9472a44325 100644 GIT binary patch delta 322 zcmY*Uu}Z^G6utMomzOj)4pNAr;-Zs_PEJLrc6CsPYCuZirJ<#X`Vzt61qVSK6e)3Z zNH+ZdCrAIH;1~D{4&IKQ%Q^R+bMJDWYd3<_@i{;hygr0rRuQU`mcOAnnkuF(p%i6h6zyTwFi-f1(*{58@0AF27J>YYIavP&|b#o2kexg&i!$kirTQ1FB^% zVajGJ$^g+dKwUK~B|tGC&6>iJ!ZC+UE|{T)xrPy_q^pJzECC@|QkZ~B*eA0vEtY1B z;>;~hcS%hPE-AXjo03|PnwOH9m+o7fUOag-lPaUk?REx4p;@)IUiM(N31%r%S( kliQhWG<2f(nV4r0whu-jY`N&e!y%40M8XJWdHyG diff --git a/mros2_header_generator/header_generator.py b/mros2_header_generator/header_generator.py index 1cef386..131e9ba 100644 --- a/mros2_header_generator/header_generator.py +++ b/mros2_header_generator/header_generator.py @@ -9,7 +9,8 @@ msgs = [] deps = [] -dependingMsgs = [] +dependingFileNames = [] +dependingMsgs = {} arg = sys.argv if(len(arg) == 1): @@ -33,13 +34,15 @@ def main(): jsonData = json.load(f) for dep in jsonData['dependingMsgs']: - dep = toSnakeCase(dep.strip()[:-4].replace('/','::')) + '.hpp' - deps.append(dep) + dep = toSnakeCase(dep.strip()[:-4]) + '.hpp' + depArr = dep.split('/') + depArr[-1] = depArr[-1][1:] + dependingFileNames.append('/'.join(depArr)) for dep in jsonData['dependingMsgs']: depArr = dep.strip().split('/') depArr[2] = depArr[2].rstrip('.msg') - dependingMsgs.append(depArr[2]) + dependingMsgs[depArr[2]] = '::'.join(depArr) for line in jsonData['includingMsgs']: line = line.strip() @@ -49,7 +52,7 @@ def main(): for msg in msgs: env = Environment(loader=FileSystemLoader(appDir + '/mros2_header_generator')) template = env.get_template('header_template.tpl') - datatext = template.render({"msg": msg, "deps": deps}) + datatext = template.render({"msg": msg, "dependingFileNames": dependingFileNames}) print(datatext) msgPkgPath = msgIncludePath + msg['pkg'] if not(os.path.isdir(msgPkgPath)): diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 6f6f1de..2eb3f95 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -5,9 +5,9 @@ #include #include -{%for dep in deps%} -#include {{dep}} -{%endfor} +{%for dependingFileName in dependingFileNames%} +#include "{{dependingFileName}}" +{%endfor%} using namespace std; @@ -27,7 +27,7 @@ public: uint8_t copyToBuf(uint8_t *addrPtr) { {%for def_data in msg.def %} - {% if def_data.isCustom%} + {% if def_data.isCustomType%} cntPub += {{def_data.typeName}}.copyToBuf(addrPtr); {% elif def_data.isArray%} @@ -101,7 +101,7 @@ public: uint8_t copyFromBuf(const uint8_t *rbuf) { {% for def_data in msg.def %} - {% if def_data.isCustom%} + {% if def_data.isCustomType%} cntSub += {{def_data.typeName}}.copyFromBuf(rbuf); {% elif def_data.isArray%} diff --git a/mros2_header_generator/msg_def_generator.py b/mros2_header_generator/msg_def_generator.py index 4b10c3f..4961e52 100644 --- a/mros2_header_generator/msg_def_generator.py +++ b/mros2_header_generator/msg_def_generator.py @@ -54,10 +54,10 @@ def msgDefGenerator(msgDefStr, dependingMsgs): elif msgType in dependingMsgs: return { - 'rosType': msgType, - 'cppType': msgType, + 'rosType': dependingMsgs[msgType], + 'cppType': dependingMsgs[msgType], 'typeName': msgName, - 'size': msgSizes[msgType], + 'size': 0, 'isArray': isArray, 'isCustomType': True } diff --git a/mros2_msgs/geometry_msgs/msg/twist.hpp b/mros2_msgs/geometry_msgs/msg/twist.hpp new file mode 100644 index 0000000..6691ca2 --- /dev/null +++ b/mros2_msgs/geometry_msgs/msg/twist.hpp @@ -0,0 +1,94 @@ +#ifndef _GEOMETRY_MSGS_MSG_TWIST_H +#define _GEOMETRY_MSGS_MSG_TWIST_H + +#include +#include +#include + + +#include "geometry_msgs/msg/vector3.hpp" + + +using namespace std; + +namespace geometry_msgs +{ +namespace msg +{ +class Twist +{ +public: + uint8_t cntPub = 0; + uint8_t cntSub = 0; + + geometry_msgs::msg::Vector3 linear +; + + geometry_msgs::msg::Vector3 angular; + + + uint8_t copyToBuf(uint8_t *addrPtr) + { + + + cntPub += linear +.copyToBuf(addrPtr); + + + + + cntPub += angular.copyToBuf(addrPtr); + + + + + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + + return cntPub; + } + + uint8_t copyFromBuf(const uint8_t *rbuf) { + + + cntSub += linear +.copyFromBuf(rbuf); + + + + + cntSub += angular.copyFromBuf(rbuf); + + + + + return cntSub; + } + + uint8_t getTotalSize(){ + return cntPub ; + } + +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 From ad303c37c0c23259e00368268b8a36ae365938be Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 23 Dec 2021 13:55:00 +0900 Subject: [PATCH 54/70] twist app worked --- mros2_msgs/geometry_msgs/msg/vector3.hpp | 10 ++++++++-- src/mros2.cpp | 6 ++++++ 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/mros2_msgs/geometry_msgs/msg/vector3.hpp b/mros2_msgs/geometry_msgs/msg/vector3.hpp index f285ed6..ddc398b 100644 --- a/mros2_msgs/geometry_msgs/msg/vector3.hpp +++ b/mros2_msgs/geometry_msgs/msg/vector3.hpp @@ -5,6 +5,8 @@ #include #include + + using namespace std; namespace geometry_msgs @@ -26,7 +28,7 @@ class Vector3 double z; - void copyToBuf(uint8_t *addrPtr) + uint8_t copyToBuf(uint8_t *addrPtr) { @@ -105,9 +107,11 @@ class Vector3 } cntPub += 4-(cntPub%4); } + + return cntPub; } - void copyFromBuf(const uint8_t *rbuf) { + uint8_t copyFromBuf(const uint8_t *rbuf) { if (cntSub%4 >0 && 8 > 1){ @@ -168,6 +172,8 @@ class Vector3 cntSub += 8; + + return cntSub; } uint8_t getTotalSize(){ diff --git a/src/mros2.cpp b/src/mros2.cpp index 0a81e07..41a5ae1 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -28,6 +28,7 @@ #include "health_msgs/msg/health.hpp" #include "location_msgs/msg/location.hpp" #include "geometry_msgs/msg/vector3.hpp" +#include "geometry_msgs/msg/twist.hpp" #include "mix_msgs/msg/mix.hpp" #ifndef __MBED__ @@ -395,6 +396,11 @@ template mros2::Subscriber mros2::Node::create_subscription(std::string topic_na template void mros2::Publisher::publish(geometry_msgs::msg::Vector3 &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(geometry_msgs::msg::Twist*)); +template void mros2::Publisher::publish(geometry_msgs::msg::Twist &msg); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); + template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(mix_msgs::msg::Mix*)); template void mros2::Publisher::publish(mix_msgs::msg::Mix &msg); From 32db7448feeee9ae52a97e6555df4d82f1c0cd8a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Tue, 28 Dec 2021 02:33:52 +0900 Subject: [PATCH 55/70] WIP: cleaning compile flow etc. --- .../msg_data_generator.cpython-37.pyc | Bin 776 -> 829 bytes .../msg_def_generator.cpython-37.pyc | Bin 1052 -> 1052 bytes mros2_header_generator/header_generator.py | 71 +++--- mros2_header_generator/header_template.tpl | 2 +- mros2_header_generator/msg_data_generator.py | 9 +- mros2_header_generator/msg_def_generator.py | 9 +- mros2_msgs/TEST.hpp | 23 -- mros2_msgs/geometry_msgs/msg/twist.hpp | 94 ------- mros2_msgs/geometry_msgs/msg/vector3.hpp | 200 --------------- mros2_msgs/health_msgs/msg/health.hpp | 191 -------------- mros2_msgs/location_msgs/msg/location.hpp | 194 -------------- mros2_msgs/mix_msgs/msg/mix.hpp | 239 ------------------ mros2_msgs/std_msgs/msg/header.hpp | 53 ++++ src/mros2.cpp | 43 +--- 14 files changed, 104 insertions(+), 1024 deletions(-) delete mode 100644 mros2_msgs/TEST.hpp delete mode 100644 mros2_msgs/geometry_msgs/msg/twist.hpp delete mode 100644 mros2_msgs/geometry_msgs/msg/vector3.hpp delete mode 100644 mros2_msgs/health_msgs/msg/health.hpp delete mode 100644 mros2_msgs/location_msgs/msg/location.hpp delete mode 100644 mros2_msgs/mix_msgs/msg/mix.hpp create mode 100644 mros2_msgs/std_msgs/msg/header.hpp diff --git a/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc index e213092706801be5de645505bf2f6c9472a44325..ac11d9dc709f869eb243bcb7cc37e05540561730 100644 GIT binary patch delta 395 zcmY+AF-yZh6vyw~U9L%_1xss5EnNgb1nn$>sMQW)aTK9L@M4IzAzm|xTwGk-q-1qS z7C(egoc#*-4P0~-oV;jJe8>CW@BjD@k9WtWtg=%s+enxB@!}52q;ee4!s@bSy{rH6 zla?5rLxv@AXdsCiCZEyy7;rOAndGKd;ob>3I_N3Fy)#wiUgU*52>tzF==F7OoDM)PS&9yW&{NEf zqHZ8m`8U@MPGeD=Dt(7#aMA^Qv)a}cK}|msDA>@(6{uncOtTFbOd=MGRj8Otr?^!w MjBx%-GkwF&FLEPW0RR91 delta 392 zcmYL_%}T>S6ov2nBu#2t5UjD#l?$OR)CVYne~3tnP!KIhDa;tEZCWQ0L?#M$<4#Ik zx=41ugexCF*8@I)uOKd*se%{o;k(1-+{3)s_jYNsSj>a2_~W(vjVGm}V{ArUIo+=A zE#2X>mI%&JiX~)7JVrzULu34~7GsH1nn6r_i0M^Tl5IGdi!Di0E3%~}^;U)uTr5xC z%C{P0TiT3NAaxndkl{M~zG$^7f1KtsLwcSPW%iTRf@p^2BzV9a560*YF=#=}Z7L+b z(CVj2eoa>fp)Zienx$ya9dJcP=f0xNjmA#CO%C>V6=D1|86!ELQ?%a;xGFL};sNUg zek1h5WDzeRFWJL4ZdLmLDxgC0j?0G*5fx)hh557gtiIIIRvn)QTy&$M5GKNOM|g(* XWkHY1sSk$ivd`B|ypU>fP=WpcXtrB3 diff --git a/mros2_header_generator/__pycache__/msg_def_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/msg_def_generator.cpython-37.pyc index b933cabf880bacc6a04d125dc0cfd0ebe06c5658..5d047cd85d4e1566e0e8c4021d9628a26b39473e 100644 GIT binary patch delta 30 kcmbQkF^7ZOiI 1: + dependingLst = msgLst[1:] + + for dep in dependingLst: + dep = toSnakeCase(dep.strip()[:-4]) + '.hpp' + depArr = dep.split('/') + if depArr[-1][0] == '_': + depArr[-1] = depArr[-1][1:] + dependingFileNames.append('/'.join(depArr)) - for dep in jsonData['dependingMsgs']: - depArr = dep.strip().split('/') - depArr[2] = depArr[2].rstrip('.msg') - dependingMsgs[depArr[2]] = '::'.join(depArr) - - for line in jsonData['includingMsgs']: - line = line.strip() - msgs.append(msgDataGenerator(line, dependingMsgs)) # generate message data of the custom type + for dep in dependingLst: + depArr = dep.strip().split('/') + depArr[2] = depArr[2].rstrip('.msg') + dependingDict[depArr[2]] = '::'.join(depArr) + + line = includingMsgs.strip() + msgs.append(msgDataGenerator(line, appDir, dependingDict, dependingFileNames)) # generate header file for mros2 for msg in msgs: - env = Environment(loader=FileSystemLoader(appDir + '/mros2_header_generator')) + env = Environment(loader=FileSystemLoader(mros2Dir + '/mros2_header_generator')) template = env.get_template('header_template.tpl') - datatext = template.render({"msg": msg, "dependingFileNames": dependingFileNames}) - print(datatext) - msgPkgPath = msgIncludePath + msg['pkg'] + datatext = template.render({ "msg": msg }) + + msgPkgPath = "mros2_msgs" + "/" + msg['pkg'] + + if not(os.path.isdir("mros2_msgs")): + os.mkdir("mros2_msgs") if not(os.path.isdir(msgPkgPath)): os.mkdir(msgPkgPath) + if not(os.path.isdir(msgPkgPath + "/msg")): os.mkdir(msgPkgPath + "/msg") + with open(os.path.join(msgPkgPath, "msg", toSnakeCase(msg['name']) + ".hpp"), "wb") as f: f.write(datatext.encode('utf-8')) if __name__ == "__main__": + msgs = [] main() \ No newline at end of file diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 2eb3f95..d037160 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -5,7 +5,7 @@ #include #include -{%for dependingFileName in dependingFileNames%} +{%for dependingFileName in msg.dependingFileNames%} #include "{{dependingFileName}}" {%endfor%} diff --git a/mros2_header_generator/msg_data_generator.py b/mros2_header_generator/msg_data_generator.py index d2ed4e4..053ae02 100644 --- a/mros2_header_generator/msg_data_generator.py +++ b/mros2_header_generator/msg_data_generator.py @@ -3,13 +3,13 @@ # generate message data is mainly about the name, definition of the type -def msgDataGenerator(line, dependingMsgs): - if os.path.isfile(line): - with open(line, 'r') as m_f: +def msgDataGenerator(line, appDir, dependingDict, dependingFileNames): + if os.path.isfile(appDir + "/" + line): + with open(appDir + "/" + line, 'r') as m_f: arr = m_f.readlines() msgDef = [] for m_line in arr: - msgDef.append(msgDefGenerator(m_line, dependingMsgs)) + msgDef.append(msgDefGenerator(m_line, dependingDict)) lineArr = line.strip().split('/') lineArr[2] = lineArr[2].rstrip('.msg') @@ -19,6 +19,7 @@ def msgDataGenerator(line, dependingMsgs): 'NAME': lineArr[2].upper(), 'PKG': lineArr[0].upper(), 'def': msgDef, + 'dependingFileNames': dependingFileNames, } else: raise Exception('msg header file "' + line + '" not found.') \ No newline at end of file diff --git a/mros2_header_generator/msg_def_generator.py b/mros2_header_generator/msg_def_generator.py index 4961e52..d70f917 100644 --- a/mros2_header_generator/msg_def_generator.py +++ b/mros2_header_generator/msg_def_generator.py @@ -28,11 +28,10 @@ # generate detail data of type definition -def msgDefGenerator(msgDefStr, dependingMsgs): +def msgDefGenerator(msgDefStr, dependingDict): # split the type def and name of the type ('string name' -> ['string', 'name']) msgDefArr = msgDefStr.split(' ') - print(msgDefArr) msgType = msgDefArr[0] # each type (ex. string, int8, float32, ...) msgName = msgDefArr[1] # name of each type (ex. name, height, weight, ...) isArray = False @@ -52,10 +51,10 @@ def msgDefGenerator(msgDefStr, dependingMsgs): 'isCustomType': False } - elif msgType in dependingMsgs: + elif msgType in dependingDict: return { - 'rosType': dependingMsgs[msgType], - 'cppType': dependingMsgs[msgType], + 'rosType': dependingDict[msgType], + 'cppType': dependingDict[msgType], 'typeName': msgName, 'size': 0, 'isArray': isArray, diff --git a/mros2_msgs/TEST.hpp b/mros2_msgs/TEST.hpp deleted file mode 100644 index e0148ff..0000000 --- a/mros2_msgs/TEST.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#include - -class TEST -{ -public: - static std::string getTypeName(); - std::string data; -private: - std::string type_name = "TEST"; -}; - -namespace message_traits -{ - -template<> -struct TypeName { - static const char* value() - { - return "TEST"; - } -}; - -} diff --git a/mros2_msgs/geometry_msgs/msg/twist.hpp b/mros2_msgs/geometry_msgs/msg/twist.hpp deleted file mode 100644 index 6691ca2..0000000 --- a/mros2_msgs/geometry_msgs/msg/twist.hpp +++ /dev/null @@ -1,94 +0,0 @@ -#ifndef _GEOMETRY_MSGS_MSG_TWIST_H -#define _GEOMETRY_MSGS_MSG_TWIST_H - -#include -#include -#include - - -#include "geometry_msgs/msg/vector3.hpp" - - -using namespace std; - -namespace geometry_msgs -{ -namespace msg -{ -class Twist -{ -public: - uint8_t cntPub = 0; - uint8_t cntSub = 0; - - geometry_msgs::msg::Vector3 linear -; - - geometry_msgs::msg::Vector3 angular; - - - uint8_t copyToBuf(uint8_t *addrPtr) - { - - - cntPub += linear -.copyToBuf(addrPtr); - - - - - cntPub += angular.copyToBuf(addrPtr); - - - - - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - - return cntPub; - } - - uint8_t copyFromBuf(const uint8_t *rbuf) { - - - cntSub += linear -.copyFromBuf(rbuf); - - - - - cntSub += angular.copyFromBuf(rbuf); - - - - - return cntSub; - } - - uint8_t getTotalSize(){ - return cntPub ; - } - -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 deleted file mode 100644 index ddc398b..0000000 --- a/mros2_msgs/geometry_msgs/msg/vector3.hpp +++ /dev/null @@ -1,200 +0,0 @@ -#ifndef _GEOMETRY_MSGS_MSG_VECTOR3_H -#define _GEOMETRY_MSGS_MSG_VECTOR3_H - -#include -#include -#include - - - -using namespace std; - -namespace geometry_msgs -{ -namespace msg -{ -class Vector3 -{ -public: - uint8_t cntPub = 0; - uint8_t cntSub = 0; - - double x -; - - double y -; - - double z; - - - uint8_t copyToBuf(uint8_t *addrPtr) - { - - - if (cntPub%4 >0 && 8 > 1){ - if ((8 <= (4-(cntPub%4)))&&(8==2)){ - for (int i=0; i<(4-(cntPub%4))-8; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-8; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&x -,8); - addrPtr += 8; - cntPub += 8; - - - - - if (cntPub%4 >0 && 8 > 1){ - if ((8 <= (4-(cntPub%4)))&&(8==2)){ - for (int i=0; i<(4-(cntPub%4))-8; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-8; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&y -,8); - addrPtr += 8; - cntPub += 8; - - - - - if (cntPub%4 >0 && 8 > 1){ - if ((8 <= (4-(cntPub%4)))&&(8==2)){ - for (int i=0; i<(4-(cntPub%4))-8; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-8; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&z,8); - addrPtr += 8; - cntPub += 8; - - - - - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - - return cntPub; - } - - uint8_t copyFromBuf(const uint8_t *rbuf) { - - - if (cntSub%4 >0 && 8 > 1){ - if ((8 <= (4-(cntSub%4)))&&(8==2)){ - for (int i=0; i<(4-(cntSub%4))-8; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-8; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&x -,rbuf,8); - rbuf += 8; - cntSub += 8; - - - - if (cntSub%4 >0 && 8 > 1){ - if ((8 <= (4-(cntSub%4)))&&(8==2)){ - for (int i=0; i<(4-(cntSub%4))-8; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-8; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&y -,rbuf,8); - rbuf += 8; - cntSub += 8; - - - - if (cntSub%4 >0 && 8 > 1){ - if ((8 <= (4-(cntSub%4)))&&(8==2)){ - for (int i=0; i<(4-(cntSub%4))-8; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-8; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&z,rbuf,8); - rbuf += 8; - cntSub += 8; - - - - return cntSub; - } - - uint8_t getTotalSize(){ - return cntPub ; - } - -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 diff --git a/mros2_msgs/health_msgs/msg/health.hpp b/mros2_msgs/health_msgs/msg/health.hpp deleted file mode 100644 index 868631e..0000000 --- a/mros2_msgs/health_msgs/msg/health.hpp +++ /dev/null @@ -1,191 +0,0 @@ -#ifndef _HEALTH_MSGS_MSG_HEALTH_H -#define _HEALTH_MSGS_MSG_HEALTH_H - -#include -#include -#include - -using namespace std; - -namespace health_msgs -{ -namespace msg -{ -class Health -{ -public: - uint8_t cntPub = 0; - uint8_t cntSub = 0; - - string name -; - - uint16_t height -; - - float weight; - - - void copyToBuf(uint8_t *addrPtr) - { - - - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - uint32_t stringSize = name -.size(); - memcpy(addrPtr,&stringSize,4); - addrPtr += 4; - cntPub += 4; - memcpy(addrPtr,name -.c_str(),stringSize); - addrPtr += stringSize; - cntPub += stringSize; - - - - - if (cntPub%4 >0 && 2 > 1){ - if ((2 <= (4-(cntPub%4)))&&(2==2)){ - for (int i=0; i<(4-(cntPub%4))-2; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-2; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&height -,2); - addrPtr += 2; - cntPub += 2; - - - - - if (cntPub%4 >0 && 4 > 1){ - if ((4 <= (4-(cntPub%4)))&&(4==2)){ - for (int i=0; i<(4-(cntPub%4))-4; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-4; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&weight,4); - addrPtr += 4; - cntPub += 4; - - - - - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - - void copyFromBuf(const uint8_t *rbuf) { - - - if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - uint32_t stringSize; - memcpy(&stringSize, rbuf, 4); - rbuf += 4; - cntSub += 4; - name -.resize(stringSize); - memcpy(&name -[0],rbuf,stringSize); - rbuf += stringSize; - cntSub += stringSize; - - - - - if (cntSub%4 >0 && 2 > 1){ - if ((2 <= (4-(cntSub%4)))&&(2==2)){ - for (int i=0; i<(4-(cntSub%4))-2; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-2; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&height -,rbuf,2); - rbuf += 2; - cntSub += 2; - - - - if (cntSub%4 >0 && 4 > 1){ - if ((4 <= (4-(cntSub%4)))&&(4==2)){ - for (int i=0; i<(4-(cntSub%4))-4; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-4; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&weight,rbuf,4); - rbuf += 4; - cntSub += 4; - - - } - - uint8_t getTotalSize(){ - return cntPub ; - } - -private: - std::string type_name = "health_msgs::msg::dds_::Health"; -}; -}; -} - -namespace message_traits -{ -template<> -struct TypeName { - static const char* value() - { - return "health_msgs::msg::dds_::Health_"; - } -}; -} - -#endif \ No newline at end of file diff --git a/mros2_msgs/location_msgs/msg/location.hpp b/mros2_msgs/location_msgs/msg/location.hpp deleted file mode 100644 index 73f90ec..0000000 --- a/mros2_msgs/location_msgs/msg/location.hpp +++ /dev/null @@ -1,194 +0,0 @@ -#ifndef _LOCATION_MSGS_MSG_LOCATION_H -#define _LOCATION_MSGS_MSG_LOCATION_H - -#include -#include -#include - -using namespace std; - -namespace location_msgs -{ -namespace msg -{ -class Location -{ -public: - uint8_t cntPub = 0; - uint8_t cntSub = 0; - - int16_t x -; - - int16_t y -; - - int16_t z; - - - void copyToBuf(uint8_t *addrPtr) - { - - - if (cntPub%4 >0 && 2 > 1){ - if ((2 <= (4-(cntPub%4)))&&(2==2)){ - for (int i=0; i<(4-(cntPub%4))-2; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-2; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&x -,2); - addrPtr += 2; - cntPub += 2; - - - - - if (cntPub%4 >0 && 2 > 1){ - if ((2 <= (4-(cntPub%4)))&&(2==2)){ - for (int i=0; i<(4-(cntPub%4))-2; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-2; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&y -,2); - addrPtr += 2; - cntPub += 2; - - - - - if (cntPub%4 >0 && 2 > 1){ - if ((2 <= (4-(cntPub%4)))&&(2==2)){ - for (int i=0; i<(4-(cntPub%4))-2; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-2; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&z,2); - addrPtr += 2; - cntPub += 2; - - - - - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - - void copyFromBuf(const uint8_t *rbuf) { - - - if (cntSub%4 >0 && 2 > 1){ - if ((2 <= (4-(cntSub%4)))&&(2==2)){ - for (int i=0; i<(4-(cntSub%4))-2; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-2; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&x -,rbuf,2); - rbuf += 2; - cntSub += 2; - - - - if (cntSub%4 >0 && 2 > 1){ - if ((2 <= (4-(cntSub%4)))&&(2==2)){ - for (int i=0; i<(4-(cntSub%4))-2; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-2; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&y -,rbuf,2); - rbuf += 2; - cntSub += 2; - - - - if (cntSub%4 >0 && 2 > 1){ - if ((2 <= (4-(cntSub%4)))&&(2==2)){ - for (int i=0; i<(4-(cntSub%4))-2; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-2; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&z,rbuf,2); - rbuf += 2; - cntSub += 2; - - - } - - uint8_t getTotalSize(){ - return cntPub ; - } - -private: - std::string type_name = "location_msgs::msg::dds_::Location"; -}; -}; -} - -namespace message_traits -{ -template<> -struct TypeName { - static const char* value() - { - return "location_msgs::msg::dds_::Location_"; - } -}; -} - -#endif \ No newline at end of file diff --git a/mros2_msgs/mix_msgs/msg/mix.hpp b/mros2_msgs/mix_msgs/msg/mix.hpp deleted file mode 100644 index 2d305bc..0000000 --- a/mros2_msgs/mix_msgs/msg/mix.hpp +++ /dev/null @@ -1,239 +0,0 @@ -#ifndef _MIX_MSGS_MSG_MIX_H -#define _MIX_MSGS_MSG_MIX_H - -#include -#include -#include - -using namespace std; - -namespace mix_msgs -{ -namespace msg -{ -class Mix -{ -public: - uint8_t cntPub = 0; - uint8_t cntSub = 0; - - string name -; - - uint16_t height -; - - float weight -; - - std::vector array; - - - void copyToBuf(uint8_t *addrPtr) - { - - - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - uint32_t stringSize = name -.size(); - memcpy(addrPtr,&stringSize,4); - addrPtr += 4; - cntPub += 4; - memcpy(addrPtr,name -.c_str(),stringSize); - addrPtr += stringSize; - cntPub += stringSize; - - - - - if (cntPub%4 >0 && 2 > 1){ - if ((2 <= (4-(cntPub%4)))&&(2==2)){ - for (int i=0; i<(4-(cntPub%4))-2; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-2; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&height -,2); - addrPtr += 2; - cntPub += 2; - - - - - if (cntPub%4 >0 && 4 > 1){ - if ((4 <= (4-(cntPub%4)))&&(4==2)){ - for (int i=0; i<(4-(cntPub%4))-4; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += (4-(cntPub%4))-4; - } else { - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - memcpy(addrPtr,&weight -,4); - addrPtr += 4; - cntPub += 4; - - - - - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - uint32_t arraySize = array.size(); - memcpy(addrPtr,&arraySize,4); - addrPtr += 4; - cntPub += 4; - const uint16_t* ptr = array.data(); - for(int i=0; i0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } - - void copyFromBuf(const uint8_t *rbuf) { - - - if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - uint32_t stringSize; - memcpy(&stringSize, rbuf, 4); - rbuf += 4; - cntSub += 4; - name -.resize(stringSize); - memcpy(&name -[0],rbuf,stringSize); - rbuf += stringSize; - cntSub += stringSize; - - - - - if (cntSub%4 >0 && 2 > 1){ - if ((2 <= (4-(cntSub%4)))&&(2==2)){ - for (int i=0; i<(4-(cntSub%4))-2; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-2; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&height -,rbuf,2); - rbuf += 2; - cntSub += 2; - - - - if (cntSub%4 >0 && 4 > 1){ - if ((4 <= (4-(cntSub%4)))&&(4==2)){ - for (int i=0; i<(4-(cntSub%4))-4; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-4; - } else { - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } - memcpy(&weight -,rbuf,4); - rbuf += 4; - cntSub += 4; - - - - if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - uint32_t arraySize; - memcpy(&arraySize,rbuf,4); - rbuf += 4; - cntSub += 4; - array.reserve(arraySize); - for(int i=0;i -struct TypeName { - static const char* value() - { - return "mix_msgs::msg::dds_::Mix_"; - } -}; -} - -#endif \ No newline at end of file diff --git a/mros2_msgs/std_msgs/msg/header.hpp b/mros2_msgs/std_msgs/msg/header.hpp new file mode 100644 index 0000000..5ccc675 --- /dev/null +++ b/mros2_msgs/std_msgs/msg/header.hpp @@ -0,0 +1,53 @@ +#include + +namespace std_msgs +{ +namespace msg +{ +class Header +{ +public: + std::string getTypeName(); + std::string frame_id; + void copyToBuf(uint8_t *addrPtr) + { + uint32_t size = frame_id.size(); + memcpy(addrPtr, &size, 4); + addrPtr += 4; + memcpy(addrPtr, frame_id.c_str(),size); + addrPtr += size; + *addrPtr = 0; + } + + void copyFromBuf(const uint8_t *addrPtr) + { + uint32_t msg_size; + memcpy(&msg_size, addrPtr, 4); + addrPtr += 4; + frame_id.resize(msg_size); + memcpy(&frame_id[0], addrPtr, msg_size); + + } + + uint8_t getTotalSize() + { + return (5 + frame_id.size()); + } +private: + std::string type_name = "std_msgs::msg::dds_::Header"; +}; +}//namspace msg +}//namespace std_msgs + +namespace message_traits +{ + +template<> +struct TypeName { + static const char* value() + { + return "std_msgs::msg::dds_::Header_"; + } +}; + +} diff --git a/src/mros2.cpp b/src/mros2.cpp index 41a5ae1..75b75bb 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -14,6 +14,7 @@ #include "std_msgs/msg/char.hpp" #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/float64.hpp" +#include "std_msgs/msg/header.hpp" #include "std_msgs/msg/int8.hpp" #include "std_msgs/msg/int16.hpp" #include "std_msgs/msg/int32.hpp" @@ -25,11 +26,6 @@ #include "std_msgs/msg/u_int64.hpp" //#include "TEST.hpp" -#include "health_msgs/msg/health.hpp" -#include "location_msgs/msg/location.hpp" -#include "geometry_msgs/msg/vector3.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "mix_msgs/msg/mix.hpp" #ifndef __MBED__ /* Statement to avoid link error */ @@ -330,6 +326,11 @@ template mros2::Subscriber mros2::Node::create_subscription(std::string topic_na template void mros2::Publisher::publish(std_msgs::msg::Float64 &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Header*)); +template void mros2::Publisher::publish(std_msgs::msg::Header &msg); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); + template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Int8*)); template void mros2::Publisher::publish(std_msgs::msg::Int8 &msg); @@ -374,35 +375,3 @@ template mros2::Publisher mros2::Node::create_publisher(s template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::UInt64*)); template void mros2::Publisher::publish(std_msgs::msg::UInt64 &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -/* Work in Progress: for custom message -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(TEST*)); -template void mros2::Publisher::publish(TEST& msg); -*/ - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(health_msgs::msg::Health*)); -template void mros2::Publisher::publish(health_msgs::msg::Health &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(location_msgs::msg::Location*)); -template void mros2::Publisher::publish(location_msgs::msg::Location &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(geometry_msgs::msg::Vector3*)); -template void mros2::Publisher::publish(geometry_msgs::msg::Vector3 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(geometry_msgs::msg::Twist*)); -template void mros2::Publisher::publish(geometry_msgs::msg::Twist &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(mix_msgs::msg::Mix*)); -template void mros2::Publisher::publish(mix_msgs::msg::Mix &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - From b35da459d1122518b3c1b0b2da08e12b64280448 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Tue, 28 Dec 2021 02:53:45 +0900 Subject: [PATCH 56/70] WIP: cleaning compile flow etc. --- mros2_header_generator/header_generator.py | 6 +++--- mros2_header_generator/msg_def_generator.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/mros2_header_generator/header_generator.py b/mros2_header_generator/header_generator.py index 610924e..9c79a45 100644 --- a/mros2_header_generator/header_generator.py +++ b/mros2_header_generator/header_generator.py @@ -22,7 +22,7 @@ def main(): dependingFileNames = [] dependingDict = {} dependingLst = [] - includingMsgs = msgLst[0] + generatingMsg = msgLst[0] if len(msgLst) > 1: dependingLst = msgLst[1:] @@ -38,8 +38,8 @@ def main(): depArr[2] = depArr[2].rstrip('.msg') dependingDict[depArr[2]] = '::'.join(depArr) - line = includingMsgs.strip() - msgs.append(msgDataGenerator(line, appDir, dependingDict, dependingFileNames)) + generatingMsg = generatingMsg.strip() + msgs.append(msgDataGenerator(generatingMsg, appDir, dependingDict, dependingFileNames)) # generate header file for mros2 for msg in msgs: diff --git a/mros2_header_generator/msg_def_generator.py b/mros2_header_generator/msg_def_generator.py index d70f917..e2901dc 100644 --- a/mros2_header_generator/msg_def_generator.py +++ b/mros2_header_generator/msg_def_generator.py @@ -36,12 +36,12 @@ def msgDefGenerator(msgDefStr, dependingDict): msgName = msgDefArr[1] # name of each type (ex. name, height, weight, ...) isArray = False - # when array (ex. int8[], float32[], ...)msgType = msgType[:-2] + # when array (ex. int8[], float32[], ...) if msgType[-2:] == "[]": isArray = True msgType = msgType[:-2] - if msgType in msgCppTypes: # when primitive type of ROS2 + if msgType in msgCppTypes: # when standard type of ROS2 return { 'rosType': msgType, 'cppType': msgCppTypes[msgType], @@ -51,7 +51,7 @@ def msgDefGenerator(msgDefStr, dependingDict): 'isCustomType': False } - elif msgType in dependingDict: + elif msgType in dependingDict: # when custom type return { 'rosType': dependingDict[msgType], 'cppType': dependingDict[msgType], From 3721462a20d35ca7686f166e690036c2af5d3a20 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Wed, 29 Dec 2021 01:28:46 +0900 Subject: [PATCH 57/70] header-generator etc. modified for improving compile flow --- .../msg_data_generator.cpython-37.pyc | Bin 829 -> 829 bytes .../msg_def_generator.cpython-37.pyc | Bin 1052 -> 1095 bytes mros2_header_generator/header_generator.py | 20 +++- mros2_header_generator/header_template.tpl | 109 +++++++++++++----- mros2_header_generator/msg_def_generator.py | 17 +-- mros2_msgs/std_msgs/msg/header.hpp | 10 ++ src/mros2.cpp | 3 +- 7 files changed, 119 insertions(+), 40 deletions(-) diff --git a/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc index ac11d9dc709f869eb243bcb7cc37e05540561730..f3cec0b70529996552edce2ea1057443541afea5 100644 GIT binary patch delta 19 ZcmdnXwwH~|iIM5D1BeA|eP7t({paR-ONigD z_!nJl?qQXE01GCTtXQ+hKDRjFHix_bm3VQ|Nd-sTnP-=~yo5Z3bQtmJNt6(+{ zTzy0wpxeiZu>OaW3027*-)vj3#jMqp0@ugnZ?q@=Kv-o74IeGdk55puv>rU0cT^e| zhRfm|5nP2A3;h|zQ~aPXOMpkl89$@v8#8)Z59HeAD9+1xR8;YHu{YwxYrJxIm{*OL zk2u?}zscoxZ;Zh_-Vkp;0*Jb%=b-_dGlG<`gAJ{0|x8<`116Z&)8R2 zT-Y2QHW9@=fEaNSBuSA+KD9{G3 z&02_UMDY$N*?Ua;fscgcr8wi9m4d_)8%bn|tVoT#QsO4;4RrDkgxR6iRAb~h4ZYHe z1K+%Kxx`zgve1w{>_FG|{T?f2$C8J@1e11 zG>Tq&(l27a`YzUV7vKEI%yM;GzPfS)MX>@f8E`#yB|BVIpJa6BI^^A0nk8g&)xjxd h`F~tP+31bKjE)BVyAoA#3#e|YZbxwKDSqLB{|A 1: dependingLst = msgLst[1:] - + + ''' for dep in dependingLst: dep = toSnakeCase(dep.strip()[:-4]) + '.hpp' depArr = dep.split('/') if depArr[-1][0] == '_': depArr[-1] = depArr[-1][1:] - dependingFileNames.append('/'.join(depArr)) + dependingFileNames.append('/'.join(depArr)) + print(dependingFileNames) + ''' for dep in dependingLst: + dependingFileNames.append(dep) depArr = dep.strip().split('/') - depArr[2] = depArr[2].rstrip('.msg') + depArr[2] = depArr[2].rstrip('.hpp') + depArr[2] = toCamelCase(depArr[2]) dependingDict[depArr[2]] = '::'.join(depArr) generatingMsg = generatingMsg.strip() @@ -47,10 +55,10 @@ def main(): template = env.get_template('header_template.tpl') datatext = template.render({ "msg": msg }) - msgPkgPath = "mros2_msgs" + "/" + msg['pkg'] + msgPkgPath = "../mros2_msgs" + "/" + msg['pkg'] - if not(os.path.isdir("mros2_msgs")): - os.mkdir("mros2_msgs") + if not(os.path.isdir("../mros2_msgs")): + os.mkdir("../mros2_msgs") if not(os.path.isdir(msgPkgPath)): os.mkdir(msgPkgPath) if not(os.path.isdir(msgPkgPath + "/msg")): diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index d037160..25764a6 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -6,7 +6,7 @@ #include {%for dependingFileName in msg.dependingFileNames%} -#include "{{dependingFileName}}" +#include "../../{{dependingFileName}}" {%endfor%} using namespace std; @@ -21,7 +21,7 @@ public: uint8_t cntPub = 0; uint8_t cntSub = 0; {%for def_data in msg.def %} - {%if def_data.isArray %}std::vector<{{def_data.cppType}}>{% else %}{{def_data.cppType}}{% endif %} {{def_data.typeName}}; + {%if 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 %} uint8_t copyToBuf(uint8_t *addrPtr) @@ -50,20 +50,49 @@ public: } {% elif def_data.cppType == "string"%} - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; + { + 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); + uint32_t stringSize = {{def_data.typeName}}.size(); + memcpy(addrPtr,&stringSize,4); + addrPtr += 4; + cntPub += 4; + 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++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + + memcpy(addrPtr,&sec,4); + addrPtr += 4; + cntPub += 4; + + memcpy(addrPtr,&nanosec,4); + addrPtr += 4; + cntPub += 4; + + uint32_t stringSize = frame_id.size(); + memcpy(addrPtr,&stringSize,4); + addrPtr += 4; + cntPub += 4; + memcpy(addrPtr,frame_id.c_str(),stringSize); + addrPtr += stringSize; + cntPub += stringSize; } - uint32_t stringSize = {{def_data.typeName}}.size(); - memcpy(addrPtr,&stringSize,4); - addrPtr += 4; - cntPub += 4; - memcpy(addrPtr,{{def_data.typeName}}.c_str(),stringSize); - addrPtr += stringSize; - cntPub += stringSize; {% else %} if (cntPub%4 >0 && {{def_data.size}} > 1){ @@ -125,20 +154,48 @@ public: } {% elif def_data.cppType == "string"%} - if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; + { + if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); } - cntSub += 4-(cntSub%4); + uint32_t stringSize; + memcpy(&stringSize, rbuf, 4); + rbuf += 4; + cntSub += 4; + {{def_data.typeName}}.resize(stringSize); + memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); + rbuf += stringSize; + cntSub += stringSize; + } + + {% elif def_data.cppType == "header"%} + { + if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + memcpy(&sec,rbuf,4); + rbuf += 4; + cntSub += 4; + + memcpy(&nanosec,rbuf,4); + rbuf += 4; + cntSub += 4; + + uint32_t stringSize; + memcpy(&stringSize, rbuf, 4); + rbuf += 4; + cntSub += 4; + frame_id.resize(stringSize); + memcpy(&frame_id[0],rbuf,stringSize); + rbuf += stringSize; + cntSub += stringSize; } - uint32_t stringSize; - memcpy(&stringSize, rbuf, 4); - rbuf += 4; - cntSub += 4; - {{def_data.typeName}}.resize(stringSize); - memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); - rbuf += stringSize; - cntSub += stringSize; {% else %} if (cntSub%4 >0 && {{def_data.size}} > 1){ diff --git a/mros2_header_generator/msg_def_generator.py b/mros2_header_generator/msg_def_generator.py index e2901dc..fc4d554 100644 --- a/mros2_header_generator/msg_def_generator.py +++ b/mros2_header_generator/msg_def_generator.py @@ -11,18 +11,20 @@ "float32": "float", "float64": "double", "string": "string", + "header": "header", "bool": "uint8_t", - "byte": "uint8_t", + "byte": "uint8_t" } # size of each type msgSizes = { - "string": 1, "bool": 1, + "string": -1, "bool": 1, "int8": 1, "uint8": 1, "int16": 2, "uint16": 2, "int32": 4, "uint32": 4, "int64": 8, "uint64": 8, - "float32": 4, "float64": 8 + "float32": 4, "float64": 8, + "header": -1 } # generate detail data of type definition @@ -36,10 +38,11 @@ def msgDefGenerator(msgDefStr, dependingDict): msgName = msgDefArr[1] # name of each type (ex. name, height, weight, ...) isArray = False - # when array (ex. int8[], float32[], ...) - if msgType[-2:] == "[]": + # when array (ex. int8[], float32[], float64[36], ...) + if msgType[-1] == "]": isArray = True - msgType = msgType[:-2] + idx = msgType.index("[") + msgType = msgType[:idx] if msgType in msgCppTypes: # when standard type of ROS2 return { @@ -62,4 +65,4 @@ def msgDefGenerator(msgDefStr, dependingDict): } else: - print('type is not found') \ No newline at end of file + print(msgType+' is not found') \ No newline at end of file diff --git a/mros2_msgs/std_msgs/msg/header.hpp b/mros2_msgs/std_msgs/msg/header.hpp index 5ccc675..bf90788 100644 --- a/mros2_msgs/std_msgs/msg/header.hpp +++ b/mros2_msgs/std_msgs/msg/header.hpp @@ -8,9 +8,15 @@ class Header { public: std::string getTypeName(); + int32_t sec; + uint32_t nanosec; std::string frame_id; void copyToBuf(uint8_t *addrPtr) { + memcpy(addrPtr, &sec, 4); + addrPtr += 4; + memcpy(addrPtr, &nanosec, 4); + addrPtr += 4; uint32_t size = frame_id.size(); memcpy(addrPtr, &size, 4); addrPtr += 4; @@ -21,6 +27,10 @@ class Header void copyFromBuf(const uint8_t *addrPtr) { + memcpy(&sec, addrPtr, 4); + addrPtr += 4; + memcpy(&nanosec, addrPtr, 4); + addrPtr += 4; uint32_t msg_size; memcpy(&msg_size, addrPtr, 4); addrPtr += 4; diff --git a/src/mros2.cpp b/src/mros2.cpp index 75b75bb..7d1e08b 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -25,7 +25,6 @@ #include "std_msgs/msg/u_int32.hpp" #include "std_msgs/msg/u_int64.hpp" -//#include "TEST.hpp" #ifndef __MBED__ /* Statement to avoid link error */ @@ -375,3 +374,5 @@ template mros2::Publisher mros2::Node::create_publisher(s template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::UInt64*)); template void mros2::Publisher::publish(std_msgs::msg::UInt64 &msg); template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); + +#include "../../header_includer/header_includer.hpp" \ No newline at end of file From 7cea413628f10bf792fbf0f37da3242de84c35cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Wed, 29 Dec 2021 23:18:53 +0900 Subject: [PATCH 58/70] refactor tpl file and fix it for nested custom type --- .../msg_def_generator.cpython-37.pyc | Bin 1095 -> 1213 bytes mros2_header_generator/header_template.tpl | 158 ++++++++++++++++-- mros2_header_generator/msg_def_generator.py | 12 +- mros2_msgs/std_msgs/msg/header.hpp | 17 +- mros2_msgs/std_msgs/msg/string.hpp | 14 +- 5 files changed, 176 insertions(+), 25 deletions(-) diff --git a/mros2_header_generator/__pycache__/msg_def_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/msg_def_generator.cpython-37.pyc index 2e1721c3b61a842baaa3544545f6099f3ca64e27..0b7ee6e90710c434ef5068563e5ffb003b8e18fa 100644 GIT binary patch delta 438 zcmY+9ze)o^5XN_B_V)I|<*I$Yz5nHbfAgU^@{} zu(7lithLAk$UTD(;oJoTcbISHH~a0teyWcs*)5k#jLuO!Ke<+K$s%9vk?5*yN$N zun}eP>2N&IUG49oS&>;Vy6UGk3sRa*GE4{7&!NsE{{!usS)mViB9oq9*yB`BI;!72 zE4o)^8h#tytW6GvC9Wt07Z{V4C}LjW3B;fPxa5lju26w27-&W;%o@EvXrZZ5HT7Kb aiD~?oCSj)9mqR`7cXyCzv`zxPCR)E73tLG5 delta 319 zcmY*UJxjzu5S@?BW)tsXDFO9}R>z^gKsZtBYp$~sRw0~-?4`J3gM|q8+o*_WX(?C< z))xMQ{0IMlm6J>54DUVW?c0I*OukHZ(C?=pwKMuY8OLwg6K1iDEe-|qyYPXwsM&KxGwse=^_+uOm3X3&RW^ClQ}FCj61N_ge#gp^ zB;qbnDcP8=!8IaSn8I*n@{8eta@66SM&XAxSEDAHU!Ip$6O|YCbP)zTz&MO~FKM{6 zi&t<(xMzR7D+d6lXrvYQP+^KaGDN7cgJDB@IgvO #include +{% set break = false %} +{% for def_data in msg.def if not break%} +{%if def_data.isArray and not def_data.boundedArray%} #include +{% set break = true %} +{% endif %} +{% endfor %} + +{% set break = false %} +{% for def_data in msg.def if not break%} +{%if def_data.boundedArray %} +#include +{% set break = true %} +{% endif %} +{% endfor %} {%for dependingFileName in msg.dependingFileNames%} #include "../../{{dependingFileName}}" @@ -20,17 +34,60 @@ class {{msg.name}} public: uint8_t cntPub = 0; uint8_t cntSub = 0; + uint8_t tmpPub = 0; + uint8_t tmpSub = 0; + {%for def_data in msg.def %} - {%if 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%} + {%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 %} uint8_t copyToBuf(uint8_t *addrPtr) { {%for def_data in msg.def %} {% if def_data.isCustomType%} - cntPub += {{def_data.typeName}}.copyToBuf(addrPtr); + tmpPub = {{def_data.typeName}}.copyToBuf(addrPtr); + cntPub += tmpPub; + addrPtr += tmpPub; - {% elif def_data.isArray%} + {% elif def_data.boundedArray%} + 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; + } + 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); + } + } else if (2==4){ + if (cntPub%4 > 0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + } else if (2==8){ + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + } + 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){ for(int i=0; i<(4-(cntPub%4)) ; i++){ *addrPtr = 0; @@ -48,7 +105,6 @@ public: addrPtr += {{def_data.size}}; cntPub += {{def_data.size}}; } - {% elif def_data.cppType == "string"%} { if (cntPub%4 >0){ @@ -95,19 +151,35 @@ public: } {% else %} - if (cntPub%4 >0 && {{def_data.size}} > 1){ - if (({{def_data.size}} <= (4-(cntPub%4)))&&({{def_data.size}}==2)){ - for (int i=0; i<(4-(cntPub%4))-{{def_data.size}}; i++){ + 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; } - cntPub += (4-(cntPub%4))-{{def_data.size}}; - } else { + 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); + } + } else if ({{def_data.size}}==4){ + 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); + } + } else if ({{def_data.size}}==8){ + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); } } memcpy(addrPtr,&{{def_data.typeName}},{{def_data.size}}); @@ -131,9 +203,46 @@ public: uint8_t copyFromBuf(const uint8_t *rbuf) { {% for def_data in msg.def %} {% if def_data.isCustomType%} - cntSub += {{def_data.typeName}}.copyFromBuf(rbuf); + tmpSub = {{def_data.typeName}}.copyFromBuf(rbuf); + cntSub += tmpSub; + rbuf += tmpSub; - {% elif def_data.isArray%} + {% elif def_data.boundedArray%} + if ({{def_data.size}}==2){ + if (cntSub%4 >0 && 2 <= (4-(cntSub%4))){ + for (int i=0; i<(4-(cntSub%4))-{{def_data.size}}; i++){ + rbuf += 1; + } + cntSub += (4-(cntSub%4))-{{def_data.size}}; + } else if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } else if ({{def_data.size}}==4){ + if (cntSub%4 > 0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } else if ({{def_data.size}}==8){ + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + rbuf += 1; + } + cntSub += 8-(cntSub%8); + } + } + for(int i=0;i<{{def_data.boundedArray}};i++){ + {{def_data.cppType}} buf; + memcpy(&buf,rbuf,{{def_data.size}}); + {{def_data.typeName}}[i] = buf; + rbuf += {{def_data.size}}; + cntSub += {{def_data.size}}; + } + {% elif def_data.isArray %} if (cntSub%4 >0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ rbuf += 1; @@ -152,7 +261,6 @@ public: rbuf += {{def_data.size}}; cntSub += {{def_data.size}}; } - {% elif def_data.cppType == "string"%} { if (cntSub%4 >0){ @@ -198,18 +306,32 @@ public: } {% else %} - if (cntSub%4 >0 && {{def_data.size}} > 1){ - if (({{def_data.size}} <= (4-(cntSub%4)))&&({{def_data.size}}==2)){ + if ({{def_data.size}}==2){ + if (cntSub%4 >0 && 2 <= (4-(cntSub%4))){ for (int i=0; i<(4-(cntSub%4))-{{def_data.size}}; i++){ rbuf += 1; } - cntSub += (4-(cntSub%4))-{{def_data.size}}; - } else { + cntSub += (4-(cntSub%4))-{{def_data.size}}; + } else if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + } else if ({{def_data.size}}==4){ + if (cntSub%4 > 0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ rbuf += 1; - } + } cntSub += 4-(cntSub%4); } + } else if ({{def_data.size}}==8){ + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + rbuf += 1; + } + cntSub += 8-(cntSub%8); + } } memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); rbuf += {{def_data.size}}; diff --git a/mros2_header_generator/msg_def_generator.py b/mros2_header_generator/msg_def_generator.py index fc4d554..fa6ee4e 100644 --- a/mros2_header_generator/msg_def_generator.py +++ b/mros2_header_generator/msg_def_generator.py @@ -37,12 +37,18 @@ def msgDefGenerator(msgDefStr, dependingDict): msgType = msgDefArr[0] # each type (ex. string, int8, float32, ...) msgName = msgDefArr[1] # name of each type (ex. name, height, weight, ...) isArray = False + boundedArray = 0 # when array (ex. int8[], float32[], float64[36], ...) if msgType[-1] == "]": isArray = True - idx = msgType.index("[") - msgType = msgType[:idx] + right_idx = msgType.index("]") + left_idx = msgType.index("[") + if right_idx - left_idx > 1: + boundedArray = int(msgType[left_idx+1:right_idx]) + msgType = msgType[:left_idx] + else: + msgType = msgType[:left_idx] if msgType in msgCppTypes: # when standard type of ROS2 return { @@ -51,6 +57,7 @@ def msgDefGenerator(msgDefStr, dependingDict): 'typeName': msgName, 'size': msgSizes[msgType], 'isArray': isArray, + 'boundedArray': boundedArray, 'isCustomType': False } @@ -61,6 +68,7 @@ def msgDefGenerator(msgDefStr, dependingDict): 'typeName': msgName, 'size': 0, 'isArray': isArray, + 'boundedArray': boundedArray, 'isCustomType': True } diff --git a/mros2_msgs/std_msgs/msg/header.hpp b/mros2_msgs/std_msgs/msg/header.hpp index bf90788..99f6437 100644 --- a/mros2_msgs/std_msgs/msg/header.hpp +++ b/mros2_msgs/std_msgs/msg/header.hpp @@ -11,18 +11,30 @@ class Header int32_t sec; uint32_t nanosec; std::string frame_id; + uint8_t cntPub = 0; + void copyToBuf(uint8_t *addrPtr) { memcpy(addrPtr, &sec, 4); addrPtr += 4; + cntPub += 4; memcpy(addrPtr, &nanosec, 4); addrPtr += 4; + cntPub += 4; uint32_t size = frame_id.size(); memcpy(addrPtr, &size, 4); addrPtr += 4; + cntPub += 4; memcpy(addrPtr, frame_id.c_str(),size); addrPtr += size; - *addrPtr = 0; + cntPub += size; + if (cntPub%4 > 0){ + for(int i=0; i<(4-(size%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + cntPub += 1; + } + } } void copyFromBuf(const uint8_t *addrPtr) @@ -36,12 +48,11 @@ class Header addrPtr += 4; frame_id.resize(msg_size); memcpy(&frame_id[0], addrPtr, msg_size); - } uint8_t getTotalSize() { - return (5 + frame_id.size()); + return cntPub; } private: std::string type_name = "std_msgs::msg::dds_::Header"; diff --git a/mros2_msgs/std_msgs/msg/string.hpp b/mros2_msgs/std_msgs/msg/string.hpp index b1f83c7..7d257e9 100644 --- a/mros2_msgs/std_msgs/msg/string.hpp +++ b/mros2_msgs/std_msgs/msg/string.hpp @@ -9,14 +9,24 @@ class String public: std::string getTypeName(); std::string data; + uint8_t cntPub = 0; void copyToBuf(uint8_t *addrPtr) { uint32_t size = data.size(); memcpy(addrPtr, &size, 4); addrPtr += 4; + cntPub += 4 ; memcpy(addrPtr, data.c_str(),size); addrPtr += size; - *addrPtr = 0; + cntPub += size; + if (cntPub%4 > 0){ + for(int i=0; i<(4-(size%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + cntPub += 1; + } + } + } void copyFromBuf(const uint8_t *addrPtr) @@ -31,7 +41,7 @@ class String uint8_t getTotalSize() { - return (5 + data.size()); + return cntPub; } private: std::string type_name = "std_msgs::msg::dds_::String"; From f1bedd5a505f9bff6f2b3d2776f09ebe0fefcb71 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 30 Dec 2021 01:01:30 +0900 Subject: [PATCH 59/70] memAlign added to publish flow --- mros2_header_generator/header_template.tpl | 203 +++++++++++---------- mros2_msgs/std_msgs/msg/bool.hpp | 4 + mros2_msgs/std_msgs/msg/byte.hpp | 5 + mros2_msgs/std_msgs/msg/char.hpp | 4 + mros2_msgs/std_msgs/msg/float32.hpp | 5 + mros2_msgs/std_msgs/msg/float64.hpp | 5 + mros2_msgs/std_msgs/msg/header.hpp | 37 ++-- mros2_msgs/std_msgs/msg/int16.hpp | 4 + mros2_msgs/std_msgs/msg/int32.hpp | 4 + mros2_msgs/std_msgs/msg/int64.hpp | 4 + mros2_msgs/std_msgs/msg/int8.hpp | 4 + mros2_msgs/std_msgs/msg/string.hpp | 37 ++-- mros2_msgs/std_msgs/msg/u_int16.hpp | 4 + mros2_msgs/std_msgs/msg/u_int32.hpp | 4 + mros2_msgs/std_msgs/msg/u_int64.hpp | 4 + mros2_msgs/std_msgs/msg/u_int8.hpp | 4 + src/mros2.cpp | 1 + 17 files changed, 209 insertions(+), 124 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 5a57033..d21fc52 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -32,17 +32,18 @@ namespace msg class {{msg.name}} { public: - uint8_t cntPub = 0; - uint8_t cntSub = 0; - uint8_t tmpPub = 0; - uint8_t tmpSub = 0; + uint32_t cntPub = 0; + uint32_t cntSub = 0; {%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 %} - uint8_t copyToBuf(uint8_t *addrPtr) + uint32_t copyToBuf(uint8_t *addrPtr) { + uint32_t tmpPub = 0; + uint32_t arraySize; + uint32_t stringSize; {%for def_data in msg.def %} {% if def_data.isCustomType%} tmpPub = {{def_data.typeName}}.copyToBuf(addrPtr); @@ -64,7 +65,7 @@ public: } cntPub += 4-(cntPub%4); } - } else if (2==4){ + } else if ({{def_data.size}}==4){ if (cntPub%4 > 0){ for(int i=0; i<(4-(cntPub%4)) ; i++){ *addrPtr = 0; @@ -72,7 +73,7 @@ public: } cntPub += 4-(cntPub%4); } - } else if (2==8){ + } else if ({{def_data.size}}==8){ if (cntPub%8 > 0){ for(int i=0; i<(8-(cntPub%8)) ; i++){ *addrPtr = 0; @@ -95,10 +96,21 @@ public: } cntPub += 4-(cntPub%4); } - uint32_t arraySize = {{def_data.typeName}}.size(); + arraySize = {{def_data.typeName}}.size(); 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++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + } + const {{def_data.cppType}}* ptr = {{def_data.typeName}}.data(); for(int i=0; i0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; } - uint32_t stringSize = {{def_data.typeName}}.size(); - memcpy(addrPtr,&stringSize,4); - addrPtr += 4; - cntPub += 4; - memcpy(addrPtr,{{def_data.typeName}}.c_str(),stringSize); - addrPtr += stringSize; - cntPub += stringSize; + cntPub += 4-(cntPub%4); } + stringSize = {{def_data.typeName}}.size(); + memcpy(addrPtr,&stringSize,4); + addrPtr += 4; + cntPub += 4; + 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++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; } + cntPub += 4-(cntPub%4); + } - memcpy(addrPtr,&sec,4); - addrPtr += 4; - cntPub += 4; - - memcpy(addrPtr,&nanosec,4); - addrPtr += 4; - cntPub += 4; + memcpy(addrPtr,&sec,4); + addrPtr += 4; + cntPub += 4; - uint32_t stringSize = frame_id.size(); - memcpy(addrPtr,&stringSize,4); - addrPtr += 4; - cntPub += 4; - memcpy(addrPtr,frame_id.c_str(),stringSize); - addrPtr += stringSize; - cntPub += stringSize; - } + memcpy(addrPtr,&nanosec,4); + addrPtr += 4; + cntPub += 4; + stringSize = frame_id.size(); + memcpy(addrPtr,&stringSize,4); + addrPtr += 4; + cntPub += 4; + 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))){ @@ -189,18 +197,14 @@ public: {% endif %} {% endfor %} - if (cntPub%4 >0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - return cntPub; } - uint8_t copyFromBuf(const uint8_t *rbuf) { + uint32_t copyFromBuf(const uint8_t *rbuf) { + uint32_t tmpSub = 0; + uint32_t arraySize; + uint32_t stringSize; + {% for def_data in msg.def %} {% if def_data.isCustomType%} tmpSub = {{def_data.typeName}}.copyFromBuf(rbuf); @@ -249,10 +253,20 @@ public: } cntSub += 4-(cntSub%4); } - uint32_t arraySize; + memcpy(&arraySize,rbuf,4); rbuf += 4; cntSub += 4; + + if ({{def_data.size}}==8){ + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + rbuf += 1; + } + cntSub += 8-(cntSub%8); + } + } + {{def_data.typeName}}.reserve(arraySize); for(int i=0;i0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - uint32_t stringSize; - memcpy(&stringSize, rbuf, 4); - rbuf += 4; - cntSub += 4; - {{def_data.typeName}}.resize(stringSize); - memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); - rbuf += stringSize; - cntSub += stringSize; + if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); } + memcpy(&stringSize, rbuf, 4); + rbuf += 4; + cntSub += 4; + {{def_data.typeName}}.resize(stringSize); + memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); + rbuf += stringSize; + cntSub += stringSize; {% elif def_data.cppType == "header"%} - { - if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - memcpy(&sec,rbuf,4); - rbuf += 4; - cntSub += 4; + if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + memcpy(&sec,rbuf,4); + rbuf += 4; + cntSub += 4; - memcpy(&nanosec,rbuf,4); - rbuf += 4; - cntSub += 4; + memcpy(&nanosec,rbuf,4); + rbuf += 4; + cntSub += 4; - uint32_t stringSize; - memcpy(&stringSize, rbuf, 4); - rbuf += 4; - cntSub += 4; - frame_id.resize(stringSize); - memcpy(&frame_id[0],rbuf,stringSize); - rbuf += stringSize; - cntSub += stringSize; - } + memcpy(&stringSize, rbuf, 4); + rbuf += 4; + cntSub += 4; + frame_id.resize(stringSize); + memcpy(&frame_id[0],rbuf,stringSize); + rbuf += stringSize; + cntSub += stringSize; {% else %} if ({{def_data.size}}==2){ @@ -342,7 +350,18 @@ public: return cntSub; } - uint8_t getTotalSize(){ + void memAlign(uint8_t *addrPtr){ + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + return ; + } + + uint32_t getTotalSize(){ return cntPub ; } diff --git a/mros2_msgs/std_msgs/msg/bool.hpp b/mros2_msgs/std_msgs/msg/bool.hpp index ac4ffe1..bb1150f 100644 --- a/mros2_msgs/std_msgs/msg/bool.hpp +++ b/mros2_msgs/std_msgs/msg/bool.hpp @@ -21,6 +21,10 @@ class Bool addrPtr += 1; } + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 1; diff --git a/mros2_msgs/std_msgs/msg/byte.hpp b/mros2_msgs/std_msgs/msg/byte.hpp index c9b1bf4..550d53c 100644 --- a/mros2_msgs/std_msgs/msg/byte.hpp +++ b/mros2_msgs/std_msgs/msg/byte.hpp @@ -20,6 +20,11 @@ class Byte memcpy(&data, addrPtr, 1); addrPtr += 1; } + + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 1; diff --git a/mros2_msgs/std_msgs/msg/char.hpp b/mros2_msgs/std_msgs/msg/char.hpp index 7cbf076..988f63a 100644 --- a/mros2_msgs/std_msgs/msg/char.hpp +++ b/mros2_msgs/std_msgs/msg/char.hpp @@ -21,6 +21,10 @@ class Char addrPtr += 1; } + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 1; diff --git a/mros2_msgs/std_msgs/msg/float32.hpp b/mros2_msgs/std_msgs/msg/float32.hpp index c248e08..b88bc44 100644 --- a/mros2_msgs/std_msgs/msg/float32.hpp +++ b/mros2_msgs/std_msgs/msg/float32.hpp @@ -20,6 +20,11 @@ class Float32 memcpy(&data, addrPtr, 4); addrPtr += 4; } + + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 4; diff --git a/mros2_msgs/std_msgs/msg/float64.hpp b/mros2_msgs/std_msgs/msg/float64.hpp index eac3b10..2d8cf78 100644 --- a/mros2_msgs/std_msgs/msg/float64.hpp +++ b/mros2_msgs/std_msgs/msg/float64.hpp @@ -20,6 +20,11 @@ class Float64 memcpy(&data, addrPtr, 8); addrPtr += 8; } + + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 8; diff --git a/mros2_msgs/std_msgs/msg/header.hpp b/mros2_msgs/std_msgs/msg/header.hpp index 99f6437..5f72cf6 100644 --- a/mros2_msgs/std_msgs/msg/header.hpp +++ b/mros2_msgs/std_msgs/msg/header.hpp @@ -12,6 +12,8 @@ class Header uint32_t nanosec; std::string frame_id; uint8_t cntPub = 0; + uint32_t pubSize; + uint32_t subSize; void copyToBuf(uint8_t *addrPtr) { @@ -21,20 +23,13 @@ class Header memcpy(addrPtr, &nanosec, 4); addrPtr += 4; cntPub += 4; - uint32_t size = frame_id.size(); - memcpy(addrPtr, &size, 4); + pubSize = frame_id.size(); + memcpy(addrPtr, &pubSize, 4); addrPtr += 4; cntPub += 4; - memcpy(addrPtr, frame_id.c_str(),size); - addrPtr += size; - cntPub += size; - if (cntPub%4 > 0){ - for(int i=0; i<(4-(size%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - cntPub += 1; - } - } + memcpy(addrPtr, frame_id.c_str(),pubSize); + addrPtr += pubSize; + cntPub += pubSize; } void copyFromBuf(const uint8_t *addrPtr) @@ -43,11 +38,21 @@ class Header addrPtr += 4; memcpy(&nanosec, addrPtr, 4); addrPtr += 4; - uint32_t msg_size; - memcpy(&msg_size, addrPtr, 4); + memcpy(&subSize, addrPtr, 4); addrPtr += 4; - frame_id.resize(msg_size); - memcpy(&frame_id[0], addrPtr, msg_size); + frame_id.resize(subSize); + memcpy(&frame_id[0], addrPtr, subSize); + } + + void memAlign(uint8_t *addrPtr){ + if (cntPub%4 > 0){ + for(int i=0; i<(4-(pubSize%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + cntPub += 1; + } + } + return; } uint8_t getTotalSize() diff --git a/mros2_msgs/std_msgs/msg/int16.hpp b/mros2_msgs/std_msgs/msg/int16.hpp index 7b9e7c9..ebd689a 100644 --- a/mros2_msgs/std_msgs/msg/int16.hpp +++ b/mros2_msgs/std_msgs/msg/int16.hpp @@ -22,6 +22,10 @@ class Int16 addrPtr += 2; } + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 2; diff --git a/mros2_msgs/std_msgs/msg/int32.hpp b/mros2_msgs/std_msgs/msg/int32.hpp index a3661ae..6341ae8 100644 --- a/mros2_msgs/std_msgs/msg/int32.hpp +++ b/mros2_msgs/std_msgs/msg/int32.hpp @@ -21,6 +21,10 @@ class Int32 addrPtr += 4; } + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 4; diff --git a/mros2_msgs/std_msgs/msg/int64.hpp b/mros2_msgs/std_msgs/msg/int64.hpp index 182a657..671640b 100644 --- a/mros2_msgs/std_msgs/msg/int64.hpp +++ b/mros2_msgs/std_msgs/msg/int64.hpp @@ -21,6 +21,10 @@ class Int64 addrPtr += 8; } + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 8; diff --git a/mros2_msgs/std_msgs/msg/int8.hpp b/mros2_msgs/std_msgs/msg/int8.hpp index 9490129..30f2fcf 100644 --- a/mros2_msgs/std_msgs/msg/int8.hpp +++ b/mros2_msgs/std_msgs/msg/int8.hpp @@ -21,6 +21,10 @@ class Int8 addrPtr += 1; } + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 1; diff --git a/mros2_msgs/std_msgs/msg/string.hpp b/mros2_msgs/std_msgs/msg/string.hpp index 7d257e9..e8414a6 100644 --- a/mros2_msgs/std_msgs/msg/string.hpp +++ b/mros2_msgs/std_msgs/msg/string.hpp @@ -10,33 +10,38 @@ class String 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; cntPub += 4 ; - memcpy(addrPtr, data.c_str(),size); - addrPtr += size; - cntPub += size; - if (cntPub%4 > 0){ - for(int i=0; i<(4-(size%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - cntPub += 1; - } - } + 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){ + for(int i=0; i<(4-(pubSize%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + cntPub += 1; + } + } + return; } uint8_t getTotalSize() diff --git a/mros2_msgs/std_msgs/msg/u_int16.hpp b/mros2_msgs/std_msgs/msg/u_int16.hpp index 963d20b..cd5750c 100644 --- a/mros2_msgs/std_msgs/msg/u_int16.hpp +++ b/mros2_msgs/std_msgs/msg/u_int16.hpp @@ -21,6 +21,10 @@ class UInt16 addrPtr += 2; } + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 2; diff --git a/mros2_msgs/std_msgs/msg/u_int32.hpp b/mros2_msgs/std_msgs/msg/u_int32.hpp index 6fb25e5..b21837e 100644 --- a/mros2_msgs/std_msgs/msg/u_int32.hpp +++ b/mros2_msgs/std_msgs/msg/u_int32.hpp @@ -21,6 +21,10 @@ class UInt32 addrPtr += 4; } + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 4; diff --git a/mros2_msgs/std_msgs/msg/u_int64.hpp b/mros2_msgs/std_msgs/msg/u_int64.hpp index f01e1a2..015f3c0 100644 --- a/mros2_msgs/std_msgs/msg/u_int64.hpp +++ b/mros2_msgs/std_msgs/msg/u_int64.hpp @@ -21,6 +21,10 @@ class UInt64 addrPtr += 8; } + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 8; diff --git a/mros2_msgs/std_msgs/msg/u_int8.hpp b/mros2_msgs/std_msgs/msg/u_int8.hpp index 119f3d3..12c65c9 100644 --- a/mros2_msgs/std_msgs/msg/u_int8.hpp +++ b/mros2_msgs/std_msgs/msg/u_int8.hpp @@ -21,6 +21,10 @@ class UInt8 addrPtr += 1; } + void memAlign(uint8_t *addrPtr){ + return; + } + uint8_t getTotalSize() { return 1; diff --git a/src/mros2.cpp b/src/mros2.cpp index 7d1e08b..e1fc088 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -196,6 +196,7 @@ template void Publisher::publish(T& msg) { msg.copyToBuf(&buf[4]); + msg.memAlign(&buf[4]); pub_ptr->newChange(rtps::ChangeKind_t::ALIVE, buf, msg.getTotalSize() + 4); } From f681251e7c58d333beb4c52239dc1bc462066ce5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Fri, 31 Dec 2021 00:40:56 +0900 Subject: [PATCH 60/70] refactor .tpl file --- mros2_header_generator/header_template.tpl | 247 ++++++++++----------- 1 file changed, 123 insertions(+), 124 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index d21fc52..ab7d39b 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -51,37 +51,37 @@ public: addrPtr += tmpPub; {% elif def_data.boundedArray%} - 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; - } - 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); - } - } else if ({{def_data.size}}==4){ - if (cntPub%4 > 0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } else if ({{def_data.size}}==8){ - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 8-(cntPub%8); + {%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; } + 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); + } + {% elif def_data.size==4 %} + if (cntPub%4 > 0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + {% elif def_data.size==8 %} + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + 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}}); @@ -101,15 +101,15 @@ public: addrPtr += 4; cntPub += 4; - if ({{def_data.size}}==8){ - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 8-(cntPub%8); - } + {% if def_data.size==8 %} + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); } + {% endif %} const {{def_data.cppType}}* ptr = {{def_data.typeName}}.data(); for(int i=0; i0 && 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++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } else if ({{def_data.size}}==4){ - if (cntPub%4 > 0){ - for(int i=0; i<(4-(cntPub%4)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 4-(cntPub%4); - } - } else if ({{def_data.size}}==8){ - if (cntPub%8 > 0){ - for(int i=0; i<(8-(cntPub%8)) ; i++){ - *addrPtr = 0; - addrPtr += 1; - } - cntPub += 8-(cntPub%8); + {%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; } + 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); } + {% elif def_data.size==4 %} + if (cntPub%4 > 0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + {% elif def_data.size==8 %} + if (cntPub%8 > 0){ + for(int i=0; i<(8-(cntPub%8)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 8-(cntPub%8); + } + {% endif %} memcpy(addrPtr,&{{def_data.typeName}},{{def_data.size}}); addrPtr += {{def_data.size}}; cntPub += {{def_data.size}}; @@ -212,33 +212,33 @@ public: rbuf += tmpSub; {% elif def_data.boundedArray%} - if ({{def_data.size}}==2){ - if (cntSub%4 >0 && 2 <= (4-(cntSub%4))){ - for (int i=0; i<(4-(cntSub%4))-{{def_data.size}}; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-{{def_data.size}}; - } else if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } else if ({{def_data.size}}==4){ - if (cntSub%4 > 0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); + {%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++){ + rbuf += 1; } - } else if ({{def_data.size}}==8){ - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + cntSub += (4-(cntSub%4))-{{def_data.size}}; + }else if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ rbuf += 1; - } - cntSub += 8-(cntSub%8); - } + } + cntSub += 4-(cntSub%4); } + {% elif def_data.size==4 %} + if (cntSub%4 > 0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + {% elif def_data.size==8 %} + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + rbuf += 1; + } + cntSub += 8-(cntSub%8); + } + {% endif %} for(int i=0;i<{{def_data.boundedArray}};i++){ {{def_data.cppType}} buf; memcpy(&buf,rbuf,{{def_data.size}}); @@ -253,19 +253,18 @@ public: } cntSub += 4-(cntSub%4); } - memcpy(&arraySize,rbuf,4); rbuf += 4; cntSub += 4; - if ({{def_data.size}}==8){ - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ - rbuf += 1; - } - cntSub += 8-(cntSub%8); - } + {%if def_data.size==8 %} + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + rbuf += 1; + } + cntSub += 8-(cntSub%8); } + {% endif %} {{def_data.typeName}}.reserve(arraySize); for(int i=0;i0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ rbuf += 1; - } + } cntSub += 4-(cntSub%4); } memcpy(&stringSize, rbuf, 4); @@ -294,7 +293,7 @@ public: if (cntSub%4 >0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ rbuf += 1; - } + } cntSub += 4-(cntSub%4); } memcpy(&sec,rbuf,4); @@ -314,33 +313,33 @@ public: cntSub += stringSize; {% else %} - if ({{def_data.size}}==2){ - if (cntSub%4 >0 && 2 <= (4-(cntSub%4))){ - for (int i=0; i<(4-(cntSub%4))-{{def_data.size}}; i++){ - rbuf += 1; - } - cntSub += (4-(cntSub%4))-{{def_data.size}}; - } else if (cntSub%4 >0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); - } - } else if ({{def_data.size}}==4){ - if (cntSub%4 > 0){ - for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; - } - cntSub += 4-(cntSub%4); + {%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++){ + rbuf += 1; } - } else if ({{def_data.size}}==8){ - if (cntSub%8 > 0){ - for(int i=0; i<(8-(cntSub%8)) ; i++){ + cntSub += (4-(cntSub%4))-{{def_data.size}}; + }else if (cntSub%4 >0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ rbuf += 1; - } - cntSub += 8-(cntSub%8); - } + } + cntSub += 4-(cntSub%4); + } + {% elif def_data.size==4 %} + if (cntSub%4 > 0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + {% elif def_data.size==8 %} + if (cntSub%8 > 0){ + for(int i=0; i<(8-(cntSub%8)) ; i++){ + rbuf += 1; + } + cntSub += 8-(cntSub%8); } + {% endif %} memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); rbuf += {{def_data.size}}; cntSub += {{def_data.size}}; From eb34d10bd5e2a7567601c24afa69b740b02a4748 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Fri, 31 Dec 2021 21:22:04 +0900 Subject: [PATCH 61/70] WIP: string array & bounded array 2021 Final Commit --- mros2_header_generator/header_template.tpl | 89 ++++++++++++++++++---- 1 file changed, 74 insertions(+), 15 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index ab7d39b..45e4100 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -3,25 +3,31 @@ #include #include -{% set break = false %} -{% for def_data in msg.def if not break%} -{%if def_data.isArray and not def_data.boundedArray%} +{%- set break = 0 %} +{%- for def_data in msg.def %} +{%- if def_data.isArray %} +{%- if def_data.boundedArray == 0 %} +{%- if break == 0 %} #include -{% set break = true %} -{% endif %} -{% endfor %} +{%- set break = 1 %} +{%- endif %} +{%- endif %} +{%- endif %} +{%- endfor %} -{% set break = false %} -{% for def_data in msg.def if not break%} -{%if def_data.boundedArray %} +{%- set break = 0 %} +{%- for def_data in msg.def%} +{%- if def_data.boundedArray %} +{%- if break == 0 %} #include -{% set break = true %} -{% endif %} -{% endfor %} +{%- set break = 1 %} +{%- endif %} +{%- endif %} +{%- endfor %} -{%for dependingFileName in msg.dependingFileNames%} +{%- for dependingFileName in msg.dependingFileNames%} #include "../../{{dependingFileName}}" -{%endfor%} +{%- endfor%} using namespace std; @@ -51,6 +57,7 @@ public: addrPtr += tmpPub; {% elif def_data.boundedArray%} + { {%if def_data.size==2%} if (cntPub%4 >0 && 2 <= (4-(cntPub%4))){ for (int i=0; i<(4-(cntPub%4))-2; i++){ @@ -88,7 +95,9 @@ public: addrPtr += {{def_data.size}}; cntPub += {{def_data.size}}; } + } {% elif def_data.isArray %} + { if (cntPub%4 >0){ for(int i=0; i<(4-(cntPub%4)) ; i++){ *addrPtr = 0; @@ -111,12 +120,36 @@ public: } {% endif %} + {% if def_data.cppType == "string"%} + 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); + } + stringSize = (ptr[i]).size(); + memcpy(addrPtr,&stringSize,4); + addrPtr += 4; + cntPub += 4; + memcpy(addrPtr,(ptr[i]).c_str(),stringSize); + addrPtr += stringSize; + cntPub += stringSize; + } + + {% else %} const {{def_data.cppType}}* ptr = {{def_data.typeName}}.data(); + for(int i=0; i0){ for(int i=0; i<(4-(cntPub%4)) ; i++){ @@ -212,6 +245,7 @@ public: rbuf += tmpSub; {% elif def_data.boundedArray%} + { {%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++){ @@ -246,7 +280,9 @@ public: rbuf += {{def_data.size}}; cntSub += {{def_data.size}}; } + } {% elif def_data.isArray %} + { if (cntSub%4 >0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ rbuf += 1; @@ -265,8 +301,29 @@ public: 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++){ + rbuf += 1; + } + cntSub += 4-(cntSub%4); + } + memcpy(&stringSize, rbuf, 4); + rbuf += 4; + cntSub += 4; + string buf; + buf.resize(stringSize); + memcpy(&buf[0],rbuf,stringSize); + {{def_data.typeName}}.push_back(buf); + rbuf += stringSize; + cntSub += stringSize; + } + + {% else %} for(int i=0;i0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ From f3e3a600dee6a8b14c55f2f4517fe82fceb13f5c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 2 Jan 2022 17:11:42 +0900 Subject: [PATCH 62/70] fix tpl file(memAlign func) --- mros2_header_generator/header_template.tpl | 9 +++++---- mros2_msgs/std_msgs/msg/header.hpp | 7 ++++--- mros2_msgs/std_msgs/msg/string.hpp | 5 +++-- 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 45e4100..15fbcc8 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -408,15 +408,16 @@ 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++){ *addrPtr = 0; addrPtr += 1; - } + } cntPub += 4-(cntPub%4); } - return ; + return; } uint32_t getTotalSize(){ diff --git a/mros2_msgs/std_msgs/msg/header.hpp b/mros2_msgs/std_msgs/msg/header.hpp index 5f72cf6..29ce809 100644 --- a/mros2_msgs/std_msgs/msg/header.hpp +++ b/mros2_msgs/std_msgs/msg/header.hpp @@ -44,13 +44,14 @@ class Header memcpy(&frame_id[0], addrPtr, subSize); } - void memAlign(uint8_t *addrPtr){ + void memAlign(uint8_t *addrPtr){ if (cntPub%4 > 0){ - for(int i=0; i<(4-(pubSize%4)) ; i++){ + addrPtr += cntPub; + for(int i=0; i<(4-(cntPub%4)) ; i++){ *addrPtr = 0; addrPtr += 1; - cntPub += 1; } + cntPub += 4-(cntPub%4); } return; } diff --git a/mros2_msgs/std_msgs/msg/string.hpp b/mros2_msgs/std_msgs/msg/string.hpp index e8414a6..0047ebb 100644 --- a/mros2_msgs/std_msgs/msg/string.hpp +++ b/mros2_msgs/std_msgs/msg/string.hpp @@ -35,11 +35,12 @@ class String void memAlign(uint8_t *addrPtr){ if (cntPub%4 > 0){ - for(int i=0; i<(4-(pubSize%4)) ; i++){ + addrPtr += cntPub; + for(int i=0; i<(4-(cntPub%4)) ; i++){ *addrPtr = 0; addrPtr += 1; - cntPub += 1; } + cntPub += 4-(cntPub%4); } return; } From 3670ae492bd0a666a980cface22a896fcf6497c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Wed, 5 Jan 2022 21:04:04 +0900 Subject: [PATCH 63/70] change the word rbuf to addrPtr in .tpl file --- mros2_header_generator/header_template.tpl | 80 +++++++++++----------- 1 file changed, 40 insertions(+), 40 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 15fbcc8..cf67fc3 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -233,51 +233,51 @@ public: return cntPub; } - uint32_t copyFromBuf(const uint8_t *rbuf) { + uint32_t copyFromBuf(const uint8_t *addrPtr) { uint32_t tmpSub = 0; uint32_t arraySize; uint32_t stringSize; {% for def_data in msg.def %} {% if def_data.isCustomType%} - tmpSub = {{def_data.typeName}}.copyFromBuf(rbuf); + tmpSub = {{def_data.typeName}}.copyFromBuf(addrPtr); cntSub += tmpSub; - rbuf += tmpSub; + addrPtr += tmpSub; {% elif def_data.boundedArray%} { {%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++){ - rbuf += 1; + addrPtr += 1; } cntSub += (4-(cntSub%4))-{{def_data.size}}; }else if (cntSub%4 >0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; + addrPtr += 1; } cntSub += 4-(cntSub%4); } {% elif def_data.size==4 %} if (cntSub%4 > 0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; + addrPtr += 1; } cntSub += 4-(cntSub%4); } {% elif def_data.size==8 %} if (cntSub%8 > 0){ for(int i=0; i<(8-(cntSub%8)) ; i++){ - rbuf += 1; + addrPtr += 1; } cntSub += 8-(cntSub%8); } {% endif %} for(int i=0;i<{{def_data.boundedArray}};i++){ {{def_data.cppType}} buf; - memcpy(&buf,rbuf,{{def_data.size}}); + memcpy(&buf,addrPtr,{{def_data.size}}); {{def_data.typeName}}[i] = buf; - rbuf += {{def_data.size}}; + addrPtr += {{def_data.size}}; cntSub += {{def_data.size}}; } } @@ -285,18 +285,18 @@ public: { if (cntSub%4 >0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; + addrPtr += 1; } cntSub += 4-(cntSub%4); } - memcpy(&arraySize,rbuf,4); - rbuf += 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++){ - rbuf += 1; + addrPtr += 1; } cntSub += 8-(cntSub%8); } @@ -308,27 +308,27 @@ public: for(int i=0;i0){ for(int j=0; j<(4-(cntSub%4)) ; j++){ - rbuf += 1; + addrPtr += 1; } cntSub += 4-(cntSub%4); } - memcpy(&stringSize, rbuf, 4); - rbuf += 4; + memcpy(&stringSize, addrPtr, 4); + addrPtr += 4; cntSub += 4; string buf; buf.resize(stringSize); - memcpy(&buf[0],rbuf,stringSize); + memcpy(&buf[0],addrPtr,stringSize); {{def_data.typeName}}.push_back(buf); - rbuf += stringSize; + addrPtr += stringSize; cntSub += stringSize; } {% else %} for(int i=0;i0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; + addrPtr += 1; } cntSub += 4-(cntSub%4); } - memcpy(&stringSize, rbuf, 4); - rbuf += 4; + memcpy(&stringSize, addrPtr, 4); + addrPtr += 4; cntSub += 4; {{def_data.typeName}}.resize(stringSize); - memcpy(&{{def_data.typeName}}[0],rbuf,stringSize); - rbuf += 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++){ - rbuf += 1; + addrPtr += 1; } cntSub += 4-(cntSub%4); } - memcpy(&sec,rbuf,4); - rbuf += 4; + memcpy(&sec,addrPtr,4); + addrPtr += 4; cntSub += 4; - memcpy(&nanosec,rbuf,4); - rbuf += 4; + memcpy(&nanosec,addrPtr,4); + addrPtr += 4; cntSub += 4; - memcpy(&stringSize, rbuf, 4); - rbuf += 4; + memcpy(&stringSize, addrPtr, 4); + addrPtr += 4; cntSub += 4; frame_id.resize(stringSize); - memcpy(&frame_id[0],rbuf,stringSize); - rbuf += 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++){ - rbuf += 1; + addrPtr += 1; } cntSub += (4-(cntSub%4))-{{def_data.size}}; }else if (cntSub%4 >0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; + addrPtr += 1; } cntSub += 4-(cntSub%4); } {% elif def_data.size==4 %} if (cntSub%4 > 0){ for(int i=0; i<(4-(cntSub%4)) ; i++){ - rbuf += 1; + addrPtr += 1; } cntSub += 4-(cntSub%4); } {% elif def_data.size==8 %} if (cntSub%8 > 0){ for(int i=0; i<(8-(cntSub%8)) ; i++){ - rbuf += 1; + addrPtr += 1; } cntSub += 8-(cntSub%8); } {% endif %} - memcpy(&{{def_data.typeName}},rbuf,{{def_data.size}}); - rbuf += {{def_data.size}}; + memcpy(&{{def_data.typeName}},addrPtr,{{def_data.size}}); + addrPtr += {{def_data.size}}; cntSub += {{def_data.size}}; {% endif %} {% endfor %} From 09ff0f8094e7df6ec14f5a859fa3bf6d93a7f0fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Thu, 6 Jan 2022 00:35:39 +0900 Subject: [PATCH 64/70] .tpl file fixed (include vector/array) --- mros2_header_generator/header_template.tpl | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index cf67fc3..645e1c6 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -3,24 +3,24 @@ #include #include -{%- set break = 0 %} +{%- set ns = namespace(break=0) -%} {%- for def_data in msg.def %} {%- if def_data.isArray %} {%- if def_data.boundedArray == 0 %} -{%- if break == 0 %} +{%- if ns.break == 0 %} #include -{%- set break = 1 %} +{%- set ns.break = ns.break + 1 -%} {%- endif %} {%- endif %} {%- endif %} {%- endfor %} -{%- set break = 0 %} +{%- set ns = namespace(break=0) -%} {%- for def_data in msg.def%} {%- if def_data.boundedArray %} -{%- if break == 0 %} +{%- if ns.break == 0 %} #include -{%- set break = 1 %} +{%- set ns.break = ns.break + 1 -%} {%- endif %} {%- endif %} {%- endfor %} From 3faa0f45d3940afd5334b838c5ee4fd33c4aa3b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Fri, 7 Jan 2022 20:42:23 +0900 Subject: [PATCH 65/70] header generator modified --- makefiles/Makefile.lwip | 83 ------------------ makefiles/Makefile.mros2 | 12 --- makefiles/Makefile.rtps | 47 ---------- .../header_generator.cpython-37.pyc | Bin 0 -> 2484 bytes .../msg_data_generator.cpython-37.pyc | Bin 829 -> 818 bytes .../msg_def_generator.cpython-37.pyc | Bin 1213 -> 1858 bytes mros2_header_generator/header_generator.py | 56 ++++++------ mros2_header_generator/header_template.tpl | 47 +++++++++- mros2_header_generator/msg_data_generator.py | 9 +- mros2_header_generator/msg_def_generator.py | 41 ++++++--- 10 files changed, 103 insertions(+), 192 deletions(-) delete mode 100644 makefiles/Makefile.lwip delete mode 100644 makefiles/Makefile.mros2 delete mode 100644 makefiles/Makefile.rtps create mode 100644 mros2_header_generator/__pycache__/header_generator.cpython-37.pyc diff --git a/makefiles/Makefile.lwip b/makefiles/Makefile.lwip deleted file mode 100644 index 24e47d2..0000000 --- a/makefiles/Makefile.lwip +++ /dev/null @@ -1,83 +0,0 @@ -# -# Makefile definition for lwIP stack and startup -# - -CDEFS += -DUSE_LWIP_NO_STDDEF -CDEFS += -DSTM32F767xx -CDEFS += -DosObjectsExternal - -# -# lwIP -# - -LWIPDIR := $(ROOTDIR)/STM32CubeF7/Middlewares/Third_Party/LwIP - -INCLUDES += -I$(LWIPDIR)/src -INCLUDES += -I$(LWIPDIR)/src/include -INCLUDES += -I$(LWIPDIR)/system -INCLUDES += -I$(APLDIR)/include - -APPLDIR += $(LWIPDIR)/system/OS -APPLDIR += $(LWIPDIR)/src/netif -APPLDIR += $(LWIPDIR)/src/api -APPLDIR += $(LWIPDIR)/src/core -APPLDIR += $(LWIPDIR)/src/core/ipv4 -APPLDIR += $(APLDIR)/src - -# -# startup (generated by STM32CubeIDE) -# - -APLDIR := $(ROOTDIR)/src/startup -CFGDIR := $(ROOTDIR)/src/config - -INCLUDES += -I$(APLDIR)/include - -APPLDIR += $(APLDIR) -APPLDIR += $(CFGDIR) - -APPL_COBJS += sys_arch.o -APPL_COBJS += ethernet.o -APPL_COBJS += tcpip.o -APPL_COBJS += netbuf.o -APPL_COBJS += netifapi.o -APPL_COBJS += err.o -APPL_COBJS += netdb.o -APPL_COBJS += api_msg.o -APPL_COBJS += sockets.o -APPL_COBJS += api_lib.o -APPL_COBJS += pbuf.o -APPL_COBJS += sys.o -APPL_COBJS += mem.o -APPL_COBJS += netif.o -APPL_COBJS += tcp_in.o -APPL_COBJS += def.o -APPL_COBJS += stats.o -APPL_COBJS += dns.o -APPL_COBJS += raw.o -APPL_COBJS += udp.o -APPL_COBJS += ip.o -APPL_COBJS += tcp.o -APPL_COBJS += timeouts.o -APPL_COBJS += init.o -APPL_COBJS += tcp_out.o -APPL_COBJS += memp.o -APPL_COBJS += inet_chksum.o -APPL_COBJS += ip4_addr.o -APPL_COBJS += dhcp.o -APPL_COBJS += etharp.o -APPL_COBJS += icmp.o -APPL_COBJS += ip4.o -APPL_COBJS += igmp.o -APPL_COBJS += autoip.o -APPL_COBJS += ip4_frag.o -APPL_COBJS += stm32f7xx_hal_eth.o -APPL_COBJS += stm32f7xx_hal_cortex.o -APPL_COBJS += stm32f7xx_it.o -APPL_COBJS += ethernetif.o -APPL_COBJS += ethernetif_init.o -APPL_COBJS += lwip.o -APPL_COBJS += stm32f7xx_hal_pcd.o -APPL_COBJS += stm32f7xx_hal_pcd_ex.o -APPL_COBJS += stm32f7xx_ll_usb.o -APPL_COBJS += syscalls.o diff --git a/makefiles/Makefile.mros2 b/makefiles/Makefile.mros2 deleted file mode 100644 index 3bde7ef..0000000 --- a/makefiles/Makefile.mros2 +++ /dev/null @@ -1,12 +0,0 @@ -# -# Makefile definition for mROS2 library -# - -MROS2DIR := $(ROOTDIR)/mros2 - -INCLUDES += -I$(MROS2DIR)/include -INCLUDES += -I$(MROS2DIR)/mros2_msgs - -APPLDIR += $(MROS2DIR)/src - -APPL_CXXOBJS += mros2.o diff --git a/makefiles/Makefile.rtps b/makefiles/Makefile.rtps deleted file mode 100644 index 3051d0d..0000000 --- a/makefiles/Makefile.rtps +++ /dev/null @@ -1,47 +0,0 @@ -# -# Makefile definition for embeddedRTPS stack -# - -RTPSDIR := $(ROOTDIR)/mros2/embeddedRTPS -MICROCDR_DIR := $(RTPSDIR)/thirdparty/Micro-CDR - -CDEFS += -DMROS2_USE_CUBE -CDEFS += -DMROS2_USE_EMBEDDEDRTPS - -INCLUDES += -I$(RTPSDIR)/include -INCLUDES += -I$(RTPSDIR)/include/rtps -INCLUDES += -I$(RTPSDIR)/thirdparty/lwip -INCLUDES += -I$(MICROCDR_DIR)/include -INCLUDES += -I$(MICROCDR_DIR)/src/c/types - -APPLDIR += $(RTPSDIR)/src -APPLDIR += $(RTPSDIR)/src/communication -APPLDIR += $(RTPSDIR)/src/discovery -APPLDIR += $(RTPSDIR)/src/entities -APPLDIR += $(RTPSDIR)/src/messages -APPLDIR += $(RTPSDIR)/src/storages -APPLDIR += $(MICROCDR_DIR)/src/c -APPLDIR += $(MICROCDR_DIR)/src/c/types - -APPL_CXXOBJS += UdpDriver.o -APPL_CXXOBJS += ParticipantProxyData.o -APPL_CXXOBJS += SEDPAgent.o -APPL_CXXOBJS += SPDPAgent.o -APPL_CXXOBJS += TopicData.o -APPL_CXXOBJS += Domain.o -APPL_CXXOBJS += Participant.o -APPL_CXXOBJS += StatelessReader.o -APPL_CXXOBJS += MessageReceiver.o -APPL_CXXOBJS += MessageTypes.o -APPL_CXXOBJS += HistoryCache.o -APPL_CXXOBJS += PBufWrapper.o -APPL_CXXOBJS += SimpleHistoryCache.o -APPL_CXXOBJS += rtps.o -APPL_CXXOBJS += ThreadPool.o - -APPL_COBJS += array.o -APPL_COBJS += basic.o -APPL_COBJS += sequence.o -APPL_COBJS += string.o -APPL_COBJS += common.o - diff --git a/mros2_header_generator/__pycache__/header_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/header_generator.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..a7a115e47210f1d8ef204eb53d0752a88a67665c GIT binary patch literal 2484 zcmaJ?OLH4V5T4nWR!_@zV&fDEA%uzrSPDsq1Grp3%;P{@0T+Z46~x*zk=I`BZf3`c ztXiFtEASUqN2(}(1jQ-8fSMzma)nz?bkADyg9NFjr>AFn=IfrXXFsY|O9XzGF8tvw z7YX?b2jias$_LPr6a*uT2Be+$v`zKcXdAjW+a~lzVD;>_-E-OwCEVC4v|a8pbAhyr z&|A=#n9ZEWq+MnO=0aa#MOK2o%F3()eT`4C>aHRBUDB@e>LWto)D6<8Jp}}0yG z$KeBCL}8DIQq@241HQ4JNZ$KA@)#FSDJ*-6xVKQlANCHko6wRoAQ;U6Dl;FGlw_1y zBf3T&(GebFud72x+{HvEY;zz|ah#j2hdV{LcmKXcd3;Y2GkVf;e zqWzzVz}!CjzV%hYMbg^#H$CC)NB-6}M9^B}$*zpzR!>C9l_f8UuP)tNzP>#0b4xBe z+Z^NSba}{yCnM21X=uj#&x$KJ-;c#=4Z$LEiww@m=%&}>!A&pW&6Ob9@`7ZwIa#V` z2cTLzksm5Mi349k98MyIA9gkHT$EumRX(y+b$kS&VjA>gnYO{8GmQz8m|r^1^~lVO z5hl!9AGnLnZ$DgW-)$Uow!mitT5=u)Tn=2-2;3J4Gv^P-bb-w0+X!?Jc)awlkI0S#?B{Uu7+uMhy^ZYwCja zceJ8*Xl1o|0yBT+GxWsMQ>DvgXN+1o0+w^3j3`l7?8$9q`w4?(c5fFtflq{Hrm`OjUvlN} za4TZGQOp6+d&)%=m}lj{!*VO>igS3P$-@WAogCz1IFk3JDgn~lyWQJx*vXf}>Un-R z3*N~^5QJ8YQ?x{D^t@T3GpHFAqXIp)I*cw-@gA(3ybH&w=5LspR1I*YONK^jWJXHi zwJ^uk(0`H|n*XM>q%9q+D`1@k{I^moD=-@vkCosuCp9(y9acz9=4$>IWO1jIm9a{a zzAWbT0$kSCv;fv0(Tdukm6bIAf5~U^5zJP#S1m0tM^3>BWzGMJ=6_Z5-%Z`D4z)PV zYX9N?;sO7s#{Ab8^gmP(CuI$F(SKM|)< zb9Vf2$GdVEsEuxz;?|n}yt*sBi0uV@6$?9g W8H7vof8B6UGh7q?XWY{+#PTnpP*B(a literal 0 HcmV?d00001 diff --git a/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc index f3cec0b70529996552edce2ea1057443541afea5..65b0f9bb70fc4d1274f61c9d4496562dd8314ce5 100644 GIT binary patch delta 346 zcmY+9y-LJD6ov1dOeR0Ofi096QBbTF!L_nlEY!xvX0fmk5hpIn#>FHHvP=}r1IUs} zu*g_iTG;svT1wtPd;=?QTpKT(!*|XM!`xT#EW9nxbAjf5`Q!L>;vIxoCmM_7khV$5 z3t1mBOyL|$CQ*S88so>*n6hL!yIaqgVkOs*8FZIlEhu9L4rek`@zP9%G8N`bBf{mz zY6D35?3g#2hoGHA?^Aj((xh3Ta?g)w!1fO zo{W-o7#&4PKj~B$e`Dv9(E?+>Mpaxd*cMf>k586W@xj?eSS+$t^ow0~-B_YiK#nbJ k74K{wgW`*=bOI`4|G&*qtn6+yl%qkoS-+ZaG8>!x2Wihs3jhEB delta 377 zcmY+AF-rq66vyAo<#M^x6OlqWD_wLEhdN7f(2gQ#hw9Lwn4_H3TF(<4atihXh}`Os zEPen-p&!7naFd@y5PYWv@rC^J`-MkH@+xK`+zZ11sJdV8gV9rXde1AP^5=7(*6TNl z49?LojSB0ap$-<)m(2Nd#is0WJLj4kZXtJvkc-avJ%wbO$h zC^&*+IHB{5XTeB?eT%u&QaKSAUvY~Sx8Zk((v9Y90$UOJ4+-rk@hrNKEhw+Lo|1K# z;sYw^!lr-tiLDaT;s9q&zvQE29G7A+nRJr03{^ad$0`{QJIPsAEVC_)iZ;93Xws_z zYcwQs46w6t0s>x|8&CTAiL2l;(<$Fgk)D3_1+>7U9mO-Q0)_u}@7;YdQ&)G)j}9 z7wY~>Ru^W`y%GHf7cOr66_P(7u65<=o=SvKOi^E*b8;SazN-3QeQY*wHX1ggZ~N>g z@3Z};xi*)iAw%$)Tdv>&>6))ixXXbdM;#5UU=>ZY(8e0pv4Ksp>exDLx;nOTr5GLT z;4vDb92l0CQs1YK0|I_X|ma=Bpd|ZGjYp{#m1H&4n05KCZY6mc&I-XK zwuHYyZYE~RUjo*b|jr13R^`Nen0!TF*53?T5~hy(XK|w#ai~r8aq6DJ<;N zS^}`7mU5N*CCMpYsLRq`YK62}iaKhkk*XVI9aaY{X;8pQ8Gwr4&dzt}7wi^1-<2!b zJMH21`fwU2;beCbkK)TSeS=oQ{V4hlm*p|)RI0O|Y1gjJ8vSwH55lCs7fu6onpvH% zyIL6MdgLYhd1V~$jeSuNmg`T$agbNMC=vlmMPFW(BJ#bV$c;#jgT$%178ScK_6CW} zJ1CUrI2hff96R1b#Mxi`bmgqt!MW^!uibH1e)Dl{GO2(YQkF+~be^p;`8^Q`=_R2& zPnqHe*{Z%V*H@tqHnib5p$&CSq4r~#TwWzt$CQ^;?NbD9d%t)?yY!~YWJO1ET{&D- l&{v?se%i&+R?MwQh*Mu|tGgXvu?S5tp{{3{?r?4z{{u=n_xu0= delta 463 zcmY+A%}&BV5XX1BZE1lP{6aAX5xrE5Pe42}JWB*8dOf3$9lvAxl(V+RB%D8N7k z4RlC=0VX6Ng{u_O6Wb+Vfjw6SGLS_rp(HI>o>(r06Bt^#Wf;^f9p&$@!XoQV-F zXXm9YdsRXcQ+lw)ux!hT3=`A9Ji!b-;UIG0T$$RZwCrmRB?aQj>MN%DCMQuCc-tj-@S@1e7JRup4XXIkA#=Rozl%^{!D6K z;PF;8;5)b@tcXkf@QfLXPIam*WT<=(hjoYXg7n^&kSo0OAG_Gjj7on1qn>vpKP=ow Jh+M*<`Y#o!WZVD% diff --git a/mros2_header_generator/header_generator.py b/mros2_header_generator/header_generator.py index ad333bb..ee94a0c 100644 --- a/mros2_header_generator/header_generator.py +++ b/mros2_header_generator/header_generator.py @@ -5,11 +5,10 @@ import sys import re from jinja2 import Environment, FileSystemLoader -from msg_data_generator import msgDataGenerator arg = sys.argv mros2Dir = arg[1] -appDir = arg[2] +msgDir = arg[2] def toCamelCase(string): return ''.join(x.capitalize() for x in string.split('_')) @@ -18,36 +17,12 @@ def toSnakeCase(string): return re.sub("(.[A-Z])",lambda x:x.group(1)[0] + "_" +x.group(1)[1],string).lower() def main(): + from msg_data_generator import msgDataGenerator # load msg settings & prepare information for .tpl file - with open(appDir + "/" + "msg_settings.json", 'r') as f: + with open(msgDir + "/" + "msg_settings.json", 'r') as f: jsonData = json.load(f) - for msgLst in jsonData['includingMsgs']: - dependingFileNames = [] - dependingDict = {} - dependingLst = [] - generatingMsg = msgLst[0] - if len(msgLst) > 1: - dependingLst = msgLst[1:] - - ''' - for dep in dependingLst: - dep = toSnakeCase(dep.strip()[:-4]) + '.hpp' - depArr = dep.split('/') - if depArr[-1][0] == '_': - depArr[-1] = depArr[-1][1:] - dependingFileNames.append('/'.join(depArr)) - print(dependingFileNames) - ''' - - for dep in dependingLst: - dependingFileNames.append(dep) - depArr = dep.strip().split('/') - depArr[2] = depArr[2].rstrip('.hpp') - depArr[2] = toCamelCase(depArr[2]) - dependingDict[depArr[2]] = '::'.join(depArr) - - generatingMsg = generatingMsg.strip() - msgs.append(msgDataGenerator(generatingMsg, appDir, dependingDict, dependingFileNames)) + for genMsg in jsonData['includingMsgs']: + msgs.append(msgDataGenerator(genMsg.strip())) # generate header file for mros2 for msg in msgs: @@ -66,6 +41,27 @@ def main(): with open(os.path.join(msgPkgPath, "msg", toSnakeCase(msg['name']) + ".hpp"), "wb") as f: f.write(datatext.encode('utf-8')) + +def genDepMsgHeader(genMsg): + from msg_data_generator import msgDataGenerator + msgs=[] + msgs.append(msgDataGenerator(genMsg.strip())) + for msg in msgs: + env = Environment(loader=FileSystemLoader(mros2Dir + '/mros2_header_generator')) + template = env.get_template('header_template.tpl') + datatext = template.render({ "msg": msg }) + + msgPkgPath = "../mros2_msgs" + "/" + msg['pkg'] + + if not(os.path.isdir("../mros2_msgs")): + os.mkdir("../mros2_msgs") + if not(os.path.isdir(msgPkgPath)): + os.mkdir(msgPkgPath) + if not(os.path.isdir(msgPkgPath + "/msg")): + os.mkdir(msgPkgPath + "/msg") + + with open(os.path.join(msgPkgPath, "msg", toSnakeCase(msg['name']) + ".hpp"), "wb") as f: + f.write(datatext.encode('utf-8')) if __name__ == "__main__": msgs = [] diff --git a/mros2_header_generator/header_template.tpl b/mros2_header_generator/header_template.tpl index 645e1c6..3f88434 100644 --- a/mros2_header_generator/header_template.tpl +++ b/mros2_header_generator/header_template.tpl @@ -26,7 +26,7 @@ {%- endfor %} {%- for dependingFileName in msg.dependingFileNames%} -#include "../../{{dependingFileName}}" +#include "{{dependingFileName}}" {%- endfor%} using namespace std; @@ -52,10 +52,32 @@ public: uint32_t stringSize; {%for def_data in msg.def %} {% if def_data.isCustomType%} + {% if def_data.isArray%} + { + if (cntPub%4 >0){ + for(int i=0; i<(4-(cntPub%4)) ; i++){ + *addrPtr = 0; + addrPtr += 1; + } + cntPub += 4-(cntPub%4); + } + arraySize = {{def_data.typeName}}.size(); + memcpy(addrPtr,&arraySize,4); + addrPtr += 4; + cntPub += 4; + + for(int i=0; i0){ + for(int i=0; i<(4-(cntSub%4)) ; i++){ + addrPtr += 1; + } + cntSub += 4-(cntSub%4); + } + memcpy(&arraySize,addrPtr,4); + addrPtr += 4; + cntSub += 4; + for(int i=0;i ['string', 'name']) msgDefArr = msgDefStr.split(' ') @@ -61,16 +67,23 @@ def msgDefGenerator(msgDefStr, dependingDict): 'isCustomType': False } - elif msgType in dependingDict: # when custom type - return { - 'rosType': dependingDict[msgType], - 'cppType': dependingDict[msgType], - 'typeName': msgName, - 'size': 0, - 'isArray': isArray, - 'boundedArray': boundedArray, - 'isCustomType': True - } - else: - print(msgType+' is not found') \ No newline at end of file + if os.path.isfile("custom_msgs/" + msgType + ".msg"): # when custom type + dependingFileName = toSnakeCase(msgType) + ".hpp" + depFileArr = dependingFileName.split("/") + if depFileArr[2][0] == "_": + depFileArr[2] = depFileArr[2][1:] + dependingFileName = "/".join(depFileArr) + dependingFileNames.append(dependingFileName) + genDepMsgHeader(msgType+".msg") + return { + 'rosType': msgType.replace("/","::"), + 'cppType': msgType.replace("/","::"), + 'typeName': msgName, + 'size': 0, + 'isArray': isArray, + 'boundedArray': boundedArray, + 'isCustomType': True + } + else: + print(msgType+' is not found') \ No newline at end of file From b7a09776d9781cec56c5f179cbcb3108de623f30 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 9 Jan 2022 20:35:50 +0900 Subject: [PATCH 66/70] automatically generate templates.hpp and header_includer.hpp when make app --- mros2_header_generator/header_includer.tpl | 1 + mros2_header_generator/templates.tpl | 13 +++ mros2_header_generator/templates_generator.py | 55 +++++++++++ src/mros2.cpp | 93 ------------------- 4 files changed, 69 insertions(+), 93 deletions(-) create mode 100644 mros2_header_generator/header_includer.tpl create mode 100644 mros2_header_generator/templates.tpl create mode 100644 mros2_header_generator/templates_generator.py diff --git a/mros2_header_generator/header_includer.tpl b/mros2_header_generator/header_includer.tpl new file mode 100644 index 0000000..ed76062 --- /dev/null +++ b/mros2_header_generator/header_includer.tpl @@ -0,0 +1 @@ +#include "../workspace/{{app}}/templates.hpp" \ No newline at end of file diff --git a/mros2_header_generator/templates.tpl b/mros2_header_generator/templates.tpl new file mode 100644 index 0000000..f024286 --- /dev/null +++ b/mros2_header_generator/templates.tpl @@ -0,0 +1,13 @@ +{%- for includeFile in includeFiles %} +{{includeFile}} +{%- endfor %} + +{% for pubMsgType in pubMsgTypes %} +template mros2::Publisher mros2::Node::create_publisher<{{pubMsgType}}>(std::string topic_name, int qos); +template void mros2::Publisher::publish({{pubMsgType}} &msg); +{% endfor %} + +{% for subMsgType in subMsgTypes %} +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)({{subMsgType}}*)); +template void mros2::Subscriber::callback_handler<{{subMsgType}}>(void *callee, const rtps::ReaderCacheChange &cacheChange); +{% endfor %} \ No newline at end of file diff --git a/mros2_header_generator/templates_generator.py b/mros2_header_generator/templates_generator.py new file mode 100644 index 0000000..517c864 --- /dev/null +++ b/mros2_header_generator/templates_generator.py @@ -0,0 +1,55 @@ +import os +import sys +import re +from jinja2 import Environment, FileSystemLoader + +arg = sys.argv +mros2Dir = arg[1] +app = arg[2] + +includeFiles = [] +pubMsgTypes = [] +subMsgTypes = [] + +def toSnakeCase(string): + return re.sub("(.[A-Z])",lambda x:x.group(1)[0] + "_" +x.group(1)[1],string).lower() + +def main(): + with open(app + "/app.cpp", 'r') as m_f: + arr = m_f.readlines() + for m_line in arr: + if "create_publisher" in m_line: + line = m_line.strip() + left_idx = line.index('<') + right_idx =line.index('>') + pubMsgTypes.append(line[left_idx+1:right_idx]) + + if "create_subscription" in m_line: + line = m_line.strip() + left_idx = line.index('<') + right_idx =line.index('>') + subMsgTypes.append(line[left_idx+1:right_idx]) + + pubSubUnion = list(set(pubMsgTypes + subMsgTypes)) + for msgType in pubSubUnion: + typeArr = msgType.split('::') + typeArr[2] = toSnakeCase(typeArr[2]) + includeFile = '/'.join(typeArr) + includeFile = '#include "' + includeFile + '.hpp"' + includeFiles.append(includeFile) + + + env = Environment(loader=FileSystemLoader(mros2Dir + '/mros2_header_generator')) + template = env.get_template('templates.tpl') + datatext = template.render({ "includeFiles":includeFiles, "pubMsgTypes":pubMsgTypes, "subMsgTypes":subMsgTypes }) + with open(os.path.join(app+"/templates.hpp"), "wb") as f: + f.write(datatext.encode('utf-8')) + + env = Environment(loader=FileSystemLoader(mros2Dir + '/mros2_header_generator')) + template = env.get_template('header_includer.tpl') + datatext = template.render({ "app": app }) + with open(os.path.join("../header_includer/header_includer.hpp"), "wb") as f: + f.write(datatext.encode('utf-8')) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/mros2.cpp b/src/mros2.cpp index e1fc088..d790f97 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -9,23 +9,6 @@ #include "cmsis_os.h" #endif /* __MBED__ */ -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/byte.hpp" -#include "std_msgs/msg/char.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/float64.hpp" -#include "std_msgs/msg/header.hpp" -#include "std_msgs/msg/int8.hpp" -#include "std_msgs/msg/int16.hpp" -#include "std_msgs/msg/int32.hpp" -#include "std_msgs/msg/int64.hpp" -#include "std_msgs/msg/string.hpp" -#include "std_msgs/msg/u_int8.hpp" -#include "std_msgs/msg/u_int16.hpp" -#include "std_msgs/msg/u_int32.hpp" -#include "std_msgs/msg/u_int64.hpp" - - #ifndef __MBED__ /* Statement to avoid link error */ void* __dso_handle=0; @@ -300,80 +283,4 @@ void setTrue(void* args) /* * specialize template functions */ - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Bool*)); -template void mros2::Publisher::publish(std_msgs::msg::Bool &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Byte*)); -template void mros2::Publisher::publish(std_msgs::msg::Byte &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Char*)); -template void mros2::Publisher::publish(std_msgs::msg::Char &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Float32*)); -template void mros2::Publisher::publish(std_msgs::msg::Float32 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Float64*)); -template void mros2::Publisher::publish(std_msgs::msg::Float64 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Header*)); -template void mros2::Publisher::publish(std_msgs::msg::Header &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Int8*)); -template void mros2::Publisher::publish(std_msgs::msg::Int8 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Int16*)); -template void mros2::Publisher::publish(std_msgs::msg::Int16 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Int32*)); -template void mros2::Publisher::publish(std_msgs::msg::Int32 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::Int64*)); -template void mros2::Publisher::publish(std_msgs::msg::Int64 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::String*)); -template void mros2::Publisher::publish(std_msgs::msg::String &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::UInt8*)); -template void mros2::Publisher::publish(std_msgs::msg::UInt8 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::UInt16*)); -template void mros2::Publisher::publish(std_msgs::msg::UInt16 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::UInt32*)); -template void mros2::Publisher::publish(std_msgs::msg::UInt32 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - -template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); -template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::UInt64*)); -template void mros2::Publisher::publish(std_msgs::msg::UInt64 &msg); -template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); - #include "../../header_includer/header_includer.hpp" \ No newline at end of file From 83515de5bb9ec42348727873a3d3ace81636983d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Wed, 12 Jan 2022 14:54:24 +0900 Subject: [PATCH 67/70] make calc_time branch cover the progress in main branch --- README.md | 2 +- src/mros2.cpp | 5 ----- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/README.md b/README.md index 0007e85..f22a539 100644 --- a/README.md +++ b/README.md @@ -41,4 +41,4 @@ Please let us know if you have a request for a support of board/kernel, or if yo ## License The source code of this repository itself is published under [Apache License 2.0](https://github.com/mROS-base/mros2/blob/main/LICENSE). -Please note that this repository contains [embeddedRTPS and its third party libraries](https://github.com/mROS-base/embeddedRTPS#third-party-libraries) as the submodule, and also check their Licenses. +Please note that this repository contains [embeddedRTPS and its third party libraries](https://github.com/mROS-base/embeddedRTPS#third-party-libraries) as the submodule, and also check their Licenses. \ No newline at end of file diff --git a/src/mros2.cpp b/src/mros2.cpp index d790f97..fd8ba07 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -9,11 +9,6 @@ #include "cmsis_os.h" #endif /* __MBED__ */ -#ifndef __MBED__ -/* Statement to avoid link error */ -void* __dso_handle=0; -#endif /* __MBED__ */ - namespace mros2 { From d49805316ef6640e55b15a8aad379c8e1505f6ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sat, 22 Jan 2022 17:55:32 +0900 Subject: [PATCH 68/70] mros2.cpp modified to measure mros2 each processing time --- src/mros2.cpp | 28 ++++++++++++++++++++++------ 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/src/mros2.cpp b/src/mros2.cpp index fd8ba07..f49b6a6 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -2,6 +2,7 @@ #include "mros2_user_config.h" #include +#include #ifdef __MBED__ #include "mbed.h" @@ -24,6 +25,9 @@ bool completeNodeInit = false; uint8_t endpointId = 0; uint32_t subCbArray[10]; +std::array timeArr; +uint32_t count = 0; + uint8_t buf[100]; uint8_t buf_index = 4; @@ -216,12 +220,24 @@ Subscriber Node::create_subscription(std::string topic_name, int qos, void(*fp)( template void Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange) { - T msg; - msg.copyFromBuf(&cacheChange.data[4]); - - SubscribeDataType *sub = (SubscribeDataType *)callee; - void (*fp)(intptr_t) = sub->cb_fp; - fp((intptr_t)&msg); + if (count == 201){ + for (int i=0; i< 200; i++){ + MROS2_INFO("%lu", timeArr[i]); + dly_tsk(1000); + } + MROS2_INFO("----------------"); + } else if (count > 201){ + + } else { + timeArr[count] = fch_hrt(); + T msg; + msg.copyFromBuf(&cacheChange.data[4]); + count++ ; + + SubscribeDataType *sub = (SubscribeDataType *)callee; + void (*fp)(intptr_t) = sub->cb_fp; + fp((intptr_t)&msg); + } } From b9e30703399c884f838971c211a61edf10b1a1d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 13 Feb 2022 16:13:19 +0900 Subject: [PATCH 69/70] prepared for release v0.2.4 --- .../header_generator.cpython-37.pyc | Bin 2484 -> 0 bytes .../msg_data_generator.cpython-37.pyc | Bin 818 -> 0 bytes .../msg_def_generator.cpython-37.pyc | Bin 1858 -> 0 bytes mros2_header_generator/header_generator.py | 2 +- src/mros2.cpp | 28 ++++-------------- 5 files changed, 7 insertions(+), 23 deletions(-) delete mode 100644 mros2_header_generator/__pycache__/header_generator.cpython-37.pyc delete mode 100644 mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc delete mode 100644 mros2_header_generator/__pycache__/msg_def_generator.cpython-37.pyc diff --git a/mros2_header_generator/__pycache__/header_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/header_generator.cpython-37.pyc deleted file mode 100644 index a7a115e47210f1d8ef204eb53d0752a88a67665c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2484 zcmaJ?OLH4V5T4nWR!_@zV&fDEA%uzrSPDsq1Grp3%;P{@0T+Z46~x*zk=I`BZf3`c ztXiFtEASUqN2(}(1jQ-8fSMzma)nz?bkADyg9NFjr>AFn=IfrXXFsY|O9XzGF8tvw z7YX?b2jias$_LPr6a*uT2Be+$v`zKcXdAjW+a~lzVD;>_-E-OwCEVC4v|a8pbAhyr z&|A=#n9ZEWq+MnO=0aa#MOK2o%F3()eT`4C>aHRBUDB@e>LWto)D6<8Jp}}0yG z$KeBCL}8DIQq@241HQ4JNZ$KA@)#FSDJ*-6xVKQlANCHko6wRoAQ;U6Dl;FGlw_1y zBf3T&(GebFud72x+{HvEY;zz|ah#j2hdV{LcmKXcd3;Y2GkVf;e zqWzzVz}!CjzV%hYMbg^#H$CC)NB-6}M9^B}$*zpzR!>C9l_f8UuP)tNzP>#0b4xBe z+Z^NSba}{yCnM21X=uj#&x$KJ-;c#=4Z$LEiww@m=%&}>!A&pW&6Ob9@`7ZwIa#V` z2cTLzksm5Mi349k98MyIA9gkHT$EumRX(y+b$kS&VjA>gnYO{8GmQz8m|r^1^~lVO z5hl!9AGnLnZ$DgW-)$Uow!mitT5=u)Tn=2-2;3J4Gv^P-bb-w0+X!?Jc)awlkI0S#?B{Uu7+uMhy^ZYwCja zceJ8*Xl1o|0yBT+GxWsMQ>DvgXN+1o0+w^3j3`l7?8$9q`w4?(c5fFtflq{Hrm`OjUvlN} za4TZGQOp6+d&)%=m}lj{!*VO>igS3P$-@WAogCz1IFk3JDgn~lyWQJx*vXf}>Un-R z3*N~^5QJ8YQ?x{D^t@T3GpHFAqXIp)I*cw-@gA(3ybH&w=5LspR1I*YONK^jWJXHi zwJ^uk(0`H|n*XM>q%9q+D`1@k{I^moD=-@vkCosuCp9(y9acz9=4$>IWO1jIm9a{a zzAWbT0$kSCv;fv0(Tdukm6bIAf5~U^5zJP#S1m0tM^3>BWzGMJ=6_Z5-%Z`D4z)PV zYX9N?;sO7s#{Ab8^gmP(CuI$F(SKM|)< zb9Vf2$GdVEsEuxz;?|n}yt*sBi0uV@6$?9g W8H7vof8B6UGh7q?XWY{+#PTnpP*B(a diff --git a/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/msg_data_generator.cpython-37.pyc deleted file mode 100644 index 65b0f9bb70fc4d1274f61c9d4496562dd8314ce5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 818 zcmYjPOK;Oa5T3C&&Z8-m3aL~=DD;*?>k6dyQXvX0Js={WLPeI4;m3=ffwCY4nqUS{F z7qJ0MF^7*>S{w^lfePi1oWdEMTj$o-&8bybC97a+6PBIcFYG<|aCcfOSXs-RqE_I! zRUyKsts^+1+g#@iTIuQd_-AyFXTW8v9hLay80JbT3T5CTxRD(p21aG)sb5XgfO@ zXx7_(weQ)lUmj?ii%>6fk%@#y$>2pa5?D2ZIy|q1w=9W44=au?{>p5}hF`ox{aAVrY&h(PWfvo@L z$)T5LGYQCWiFSiz3m;gwaM^OuA-#nbHf*^{$S#u$rYO@~pE`dNeAb~EW>(C9>>fGj T#!j5_@kl%|pQ#wJ2DaH>lYQDi diff --git a/mros2_header_generator/__pycache__/msg_def_generator.cpython-37.pyc b/mros2_header_generator/__pycache__/msg_def_generator.cpython-37.pyc deleted file mode 100644 index d08e8944e9990a65c303925f3bcec9d0f7c7505f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1858 zcmZWpOKTiQ5T2g*zVxzVSq8(B;3P{TEs-4A7$M@wI3^HXWM9UZWp=w)8f$iE=$<9k zG7AAa_xu1Kw2({wQRr(9LFeS8OY&8Z*LLD*=Br0nS5-YFiaDR}n z_?E$ZgI`Giw>V3!sG=-lknPkSIg!IG?r@iT+~)xgd5zb3gEx7LFG1__<=0D*%UAfS z8EbrvU%|MFLAb&{dfkdVevPlcvLc^f=Nq5_XGd1&#s%V9haIMukHz?b$bV4Bk3`Ic zjQnI=-0c-nFj+95U+qE@lE!cWn4Rr~%5bFGy}eKor@djC#RVc~lYT0+uL_xrkF|ed z0(F+Oi#2z(X98)@IM;p^#E3iFl?Y0+ySFP5G%&8AubAE2(#3wH2heUm%hJg0pB3Wb zAAA=K+6$Y2j4nbzr&TRj0WW=mfPkiCjetrg8w5>)7Qqq$RnEj2kX!nQQs4vp>KdT1 z&KYOi=FTgtwC2vaJ-2Y??%_22eCz4`&FER@KZ_}=BU;=Bl-4=gVUFb+DE|68nQd6> zl^eMRJ4s!F0+vCJkp81~{fOKw1a8F!y3AyxMz>BllY%aXxKBD3xTp-zh|?~Y`q zwl`yy?`#h5?B1Csm8Qa8Ri7RjL&&(uWOt;Fdt40PX}0n+9qy&^sL$j5PZ01g{MPhl zksXcWQ*jV0v9*_GgE&?DU@E(gqird)qb7arrP+@{c34#fO4;NsZ-CQ{_uPC8g`(P7 zOtygo!-JvPP`S*hKf+-w4sK?bHXYu~0d+ueQ)V8wXMX8$4~OcX+VUqJ%)HV;o%}hQ z1%?a3g=J8N*KzQtt}F_>bjr{aA@~^{UozvvDnpWH&bSGWZ>7>HoKs)^O0-1W(k)y= zz0${Bt-TJ8tipTlp9j2NqOiQNZq3;*Ox`WR(s{{vbKRQN${NbwLMHXH&X)ZGFFp5*x@>&h#LDCsIq?R)p8t#+c? z?lsKR*$QTjl}%{B8KEP;hPO^AH$mi0K(zAz ySHK*T{DKU=Bxn=RviH(`mvgu`%J?J|`}9nyZ2%?ObQd8%q@jmxyd9wvdjA3|JInC^ diff --git a/mros2_header_generator/header_generator.py b/mros2_header_generator/header_generator.py index ee94a0c..5ee7f38 100644 --- a/mros2_header_generator/header_generator.py +++ b/mros2_header_generator/header_generator.py @@ -21,7 +21,7 @@ def main(): # load msg settings & prepare information for .tpl file with open(msgDir + "/" + "msg_settings.json", 'r') as f: jsonData = json.load(f) - for genMsg in jsonData['includingMsgs']: + for genMsg in jsonData['pubsubMsgs']: msgs.append(msgDataGenerator(genMsg.strip())) # generate header file for mros2 diff --git a/src/mros2.cpp b/src/mros2.cpp index f49b6a6..fd8ba07 100644 --- a/src/mros2.cpp +++ b/src/mros2.cpp @@ -2,7 +2,6 @@ #include "mros2_user_config.h" #include -#include #ifdef __MBED__ #include "mbed.h" @@ -25,9 +24,6 @@ bool completeNodeInit = false; uint8_t endpointId = 0; uint32_t subCbArray[10]; -std::array timeArr; -uint32_t count = 0; - uint8_t buf[100]; uint8_t buf_index = 4; @@ -220,24 +216,12 @@ Subscriber Node::create_subscription(std::string topic_name, int qos, void(*fp)( template void Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange) { - if (count == 201){ - for (int i=0; i< 200; i++){ - MROS2_INFO("%lu", timeArr[i]); - dly_tsk(1000); - } - MROS2_INFO("----------------"); - } else if (count > 201){ - - } else { - timeArr[count] = fch_hrt(); - T msg; - msg.copyFromBuf(&cacheChange.data[4]); - count++ ; - - SubscribeDataType *sub = (SubscribeDataType *)callee; - void (*fp)(intptr_t) = sub->cb_fp; - fp((intptr_t)&msg); - } + T msg; + msg.copyFromBuf(&cacheChange.data[4]); + + SubscribeDataType *sub = (SubscribeDataType *)callee; + void (*fp)(intptr_t) = sub->cb_fp; + fp((intptr_t)&msg); } From c5faad80c7c8d1319ae0a3473dbbfd7e47cac94b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=AA=9C=E5=8E=9F=E9=99=BD=E4=B8=80=E9=83=8E?= Date: Sun, 13 Feb 2022 20:50:21 +0900 Subject: [PATCH 70/70] remove embeddedRTPS/src/rtps.cpp from make target --- CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 20a02ce..60b132e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,6 @@ target_sources(mros2 embeddedRTPS/src/communication/UdpDriver.cpp embeddedRTPS/src/messages/MessageTypes.cpp embeddedRTPS/src/messages/MessageReceiver.cpp - embeddedRTPS/src/rtps.cpp embeddedRTPS/src/discovery/TopicData.cpp embeddedRTPS/src/discovery/ParticipantProxyData.cpp embeddedRTPS/src/discovery/SEDPAgent.cpp @@ -32,4 +31,4 @@ target_sources(mros2 embeddedRTPS/thirdparty/Micro-CDR/src/c/types/string.c embeddedRTPS/thirdparty/Micro-CDR/src/c/types/sequence.c embeddedRTPS/thirdparty/Micro-CDR/src/c/types/array.c -) +) \ No newline at end of file