S Morita / mbed-mros2

Porting mros2 as an Mbed library.

Dependents:   mbed-os-example-mros2 example-mbed-mros2-sub-pose example-mbed-mros2-pub-twist example-mbed-mros2-mturtle-teleop

custom_msgs/geometry_msgs/msg/point.hpp@7:c80f65422d99, 2022-03-19 (annotated)

Committer:
smoritaemb
Date:
Sat Mar 19 09:23:37 2022 +0900
Revision:
7:c80f65422d99
Parent:
3:aaf6422e0be9 
Merge test_assortment_of_msgs branch.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smoritaemb 3:aaf6422e0be9 1 #ifndef _GEOMETRY_MSGS_MSG_POINT_H
smoritaemb 3:aaf6422e0be9 2 #define _GEOMETRY_MSGS_MSG_POINT_H
smoritaemb 3:aaf6422e0be9 3
smoritaemb 3:aaf6422e0be9 4 #include <iostream>
smoritaemb 3:aaf6422e0be9 5 #include <string>
smoritaemb 3:aaf6422e0be9 6
smoritaemb 3:aaf6422e0be9 7 using namespace std;
smoritaemb 3:aaf6422e0be9 8
smoritaemb 3:aaf6422e0be9 9 namespace geometry_msgs
smoritaemb 3:aaf6422e0be9 10 {
smoritaemb 3:aaf6422e0be9 11 namespace msg
smoritaemb 3:aaf6422e0be9 12 {
smoritaemb 3:aaf6422e0be9 13 class Point
smoritaemb 3:aaf6422e0be9 14 {
smoritaemb 3:aaf6422e0be9 15 public:
smoritaemb 3:aaf6422e0be9 16 uint32_t cntPub = 0;
smoritaemb 3:aaf6422e0be9 17 uint32_t cntSub = 0;
smoritaemb 3:aaf6422e0be9 18
smoritaemb 3:aaf6422e0be9 19
smoritaemb 3:aaf6422e0be9 20 double x
smoritaemb 3:aaf6422e0be9 21 ;
smoritaemb 3:aaf6422e0be9 22
smoritaemb 3:aaf6422e0be9 23 double y
smoritaemb 3:aaf6422e0be9 24 ;
smoritaemb 3:aaf6422e0be9 25
smoritaemb 3:aaf6422e0be9 26 double z;
smoritaemb 3:aaf6422e0be9 27
smoritaemb 3:aaf6422e0be9 28
smoritaemb 3:aaf6422e0be9 29 uint32_t copyToBuf(uint8_t *addrPtr)
smoritaemb 3:aaf6422e0be9 30 {
smoritaemb 3:aaf6422e0be9 31 uint32_t tmpPub = 0;
smoritaemb 3:aaf6422e0be9 32 uint32_t arraySize;
smoritaemb 3:aaf6422e0be9 33 uint32_t stringSize;
smoritaemb 3:aaf6422e0be9 34
smoritaemb 3:aaf6422e0be9 35
smoritaemb 3:aaf6422e0be9 36
smoritaemb 3:aaf6422e0be9 37 if (cntPub%8 > 0){
smoritaemb 3:aaf6422e0be9 38 for(int i=0; i<(8-(cntPub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 39 *addrPtr = 0;
smoritaemb 3:aaf6422e0be9 40 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 41 }
smoritaemb 3:aaf6422e0be9 42 cntPub += 8-(cntPub%8);
smoritaemb 3:aaf6422e0be9 43 }
smoritaemb 3:aaf6422e0be9 44
smoritaemb 3:aaf6422e0be9 45 memcpy(addrPtr,&x
smoritaemb 3:aaf6422e0be9 46 ,8);
smoritaemb 3:aaf6422e0be9 47 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 48 cntPub += 8;
smoritaemb 3:aaf6422e0be9 49
smoritaemb 3:aaf6422e0be9 50
smoritaemb 3:aaf6422e0be9 51
smoritaemb 3:aaf6422e0be9 52
smoritaemb 3:aaf6422e0be9 53
smoritaemb 3:aaf6422e0be9 54 if (cntPub%8 > 0){
smoritaemb 3:aaf6422e0be9 55 for(int i=0; i<(8-(cntPub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 56 *addrPtr = 0;
smoritaemb 3:aaf6422e0be9 57 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 58 }
smoritaemb 3:aaf6422e0be9 59 cntPub += 8-(cntPub%8);
smoritaemb 3:aaf6422e0be9 60 }
smoritaemb 3:aaf6422e0be9 61
smoritaemb 3:aaf6422e0be9 62 memcpy(addrPtr,&y
smoritaemb 3:aaf6422e0be9 63 ,8);
smoritaemb 3:aaf6422e0be9 64 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 65 cntPub += 8;
smoritaemb 3:aaf6422e0be9 66
smoritaemb 3:aaf6422e0be9 67
smoritaemb 3:aaf6422e0be9 68
smoritaemb 3:aaf6422e0be9 69
smoritaemb 3:aaf6422e0be9 70
smoritaemb 3:aaf6422e0be9 71 if (cntPub%8 > 0){
smoritaemb 3:aaf6422e0be9 72 for(int i=0; i<(8-(cntPub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 73 *addrPtr = 0;
smoritaemb 3:aaf6422e0be9 74 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 75 }
smoritaemb 3:aaf6422e0be9 76 cntPub += 8-(cntPub%8);
smoritaemb 3:aaf6422e0be9 77 }
smoritaemb 3:aaf6422e0be9 78
smoritaemb 3:aaf6422e0be9 79 memcpy(addrPtr,&z,8);
smoritaemb 3:aaf6422e0be9 80 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 81 cntPub += 8;
smoritaemb 3:aaf6422e0be9 82
smoritaemb 3:aaf6422e0be9 83
smoritaemb 3:aaf6422e0be9 84
smoritaemb 3:aaf6422e0be9 85
smoritaemb 3:aaf6422e0be9 86 return cntPub;
smoritaemb 3:aaf6422e0be9 87 }
smoritaemb 3:aaf6422e0be9 88
smoritaemb 3:aaf6422e0be9 89 uint32_t copyFromBuf(const uint8_t *addrPtr) {
smoritaemb 3:aaf6422e0be9 90 uint32_t tmpSub = 0;
smoritaemb 3:aaf6422e0be9 91 uint32_t arraySize;
smoritaemb 3:aaf6422e0be9 92 uint32_t stringSize;
smoritaemb 3:aaf6422e0be9 93
smoritaemb 3:aaf6422e0be9 94
smoritaemb 3:aaf6422e0be9 95
smoritaemb 3:aaf6422e0be9 96
smoritaemb 3:aaf6422e0be9 97 if (cntSub%8 > 0){
smoritaemb 3:aaf6422e0be9 98 for(int i=0; i<(8-(cntSub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 99 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 100 }
smoritaemb 3:aaf6422e0be9 101 cntSub += 8-(cntSub%8);
smoritaemb 3:aaf6422e0be9 102 }
smoritaemb 3:aaf6422e0be9 103
smoritaemb 3:aaf6422e0be9 104 memcpy(&x
smoritaemb 3:aaf6422e0be9 105 ,addrPtr,8);
smoritaemb 3:aaf6422e0be9 106 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 107 cntSub += 8;
smoritaemb 3:aaf6422e0be9 108
smoritaemb 3:aaf6422e0be9 109
smoritaemb 3:aaf6422e0be9 110
smoritaemb 3:aaf6422e0be9 111
smoritaemb 3:aaf6422e0be9 112 if (cntSub%8 > 0){
smoritaemb 3:aaf6422e0be9 113 for(int i=0; i<(8-(cntSub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 114 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 115 }
smoritaemb 3:aaf6422e0be9 116 cntSub += 8-(cntSub%8);
smoritaemb 3:aaf6422e0be9 117 }
smoritaemb 3:aaf6422e0be9 118
smoritaemb 3:aaf6422e0be9 119 memcpy(&y
smoritaemb 3:aaf6422e0be9 120 ,addrPtr,8);
smoritaemb 3:aaf6422e0be9 121 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 122 cntSub += 8;
smoritaemb 3:aaf6422e0be9 123
smoritaemb 3:aaf6422e0be9 124
smoritaemb 3:aaf6422e0be9 125
smoritaemb 3:aaf6422e0be9 126
smoritaemb 3:aaf6422e0be9 127 if (cntSub%8 > 0){
smoritaemb 3:aaf6422e0be9 128 for(int i=0; i<(8-(cntSub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 129 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 130 }
smoritaemb 3:aaf6422e0be9 131 cntSub += 8-(cntSub%8);
smoritaemb 3:aaf6422e0be9 132 }
smoritaemb 3:aaf6422e0be9 133
smoritaemb 3:aaf6422e0be9 134 memcpy(&z,addrPtr,8);
smoritaemb 3:aaf6422e0be9 135 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 136 cntSub += 8;
smoritaemb 3:aaf6422e0be9 137
smoritaemb 3:aaf6422e0be9 138
smoritaemb 3:aaf6422e0be9 139
smoritaemb 3:aaf6422e0be9 140 return cntSub;
smoritaemb 3:aaf6422e0be9 141 }
smoritaemb 3:aaf6422e0be9 142
smoritaemb 3:aaf6422e0be9 143 void memAlign(uint8_t *addrPtr){
smoritaemb 3:aaf6422e0be9 144 if (cntPub%4 > 0){
smoritaemb 3:aaf6422e0be9 145 addrPtr += cntPub;
smoritaemb 3:aaf6422e0be9 146 for(int i=0; i<(4-(cntPub%4)) ; i++){
smoritaemb 3:aaf6422e0be9 147 *addrPtr = 0;
smoritaemb 3:aaf6422e0be9 148 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 149 }
smoritaemb 3:aaf6422e0be9 150 cntPub += 4-(cntPub%4);
smoritaemb 3:aaf6422e0be9 151 }
smoritaemb 3:aaf6422e0be9 152 return;
smoritaemb 3:aaf6422e0be9 153 }
smoritaemb 3:aaf6422e0be9 154
smoritaemb 3:aaf6422e0be9 155 uint32_t getTotalSize(){
smoritaemb 3:aaf6422e0be9 156 uint32_t tmpCntPub = cntPub;
smoritaemb 3:aaf6422e0be9 157 cntPub = 0;
smoritaemb 3:aaf6422e0be9 158 return tmpCntPub ;
smoritaemb 3:aaf6422e0be9 159 }
smoritaemb 3:aaf6422e0be9 160
smoritaemb 3:aaf6422e0be9 161 private:
smoritaemb 3:aaf6422e0be9 162 std::string type_name = "geometry_msgs::msg::dds_::Point";
smoritaemb 3:aaf6422e0be9 163 };
smoritaemb 3:aaf6422e0be9 164 };
smoritaemb 3:aaf6422e0be9 165 }
smoritaemb 3:aaf6422e0be9 166
smoritaemb 3:aaf6422e0be9 167 namespace message_traits
smoritaemb 3:aaf6422e0be9 168 {
smoritaemb 3:aaf6422e0be9 169 template<>
smoritaemb 3:aaf6422e0be9 170 struct TypeName<geometry_msgs::msg::Point*> {
smoritaemb 3:aaf6422e0be9 171 static const char* value()
smoritaemb 3:aaf6422e0be9 172 {
smoritaemb 3:aaf6422e0be9 173 return "geometry_msgs::msg::dds_::Point_";
smoritaemb 3:aaf6422e0be9 174 }
smoritaemb 3:aaf6422e0be9 175 };
smoritaemb 3:aaf6422e0be9 176 }
smoritaemb 3:aaf6422e0be9 177
smoritaemb 3:aaf6422e0be9 178 #endif