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/quaternion.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_QUATERNION_H
smoritaemb 3:aaf6422e0be9 2 #define _GEOMETRY_MSGS_MSG_QUATERNION_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 Quaternion
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 double w;
smoritaemb 3:aaf6422e0be9 30
smoritaemb 3:aaf6422e0be9 31
smoritaemb 3:aaf6422e0be9 32 uint32_t copyToBuf(uint8_t *addrPtr)
smoritaemb 3:aaf6422e0be9 33 {
smoritaemb 3:aaf6422e0be9 34 uint32_t tmpPub = 0;
smoritaemb 3:aaf6422e0be9 35 uint32_t arraySize;
smoritaemb 3:aaf6422e0be9 36 uint32_t stringSize;
smoritaemb 3:aaf6422e0be9 37
smoritaemb 3:aaf6422e0be9 38
smoritaemb 3:aaf6422e0be9 39
smoritaemb 3:aaf6422e0be9 40 if (cntPub%8 > 0){
smoritaemb 3:aaf6422e0be9 41 for(int i=0; i<(8-(cntPub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 42 *addrPtr = 0;
smoritaemb 3:aaf6422e0be9 43 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 44 }
smoritaemb 3:aaf6422e0be9 45 cntPub += 8-(cntPub%8);
smoritaemb 3:aaf6422e0be9 46 }
smoritaemb 3:aaf6422e0be9 47
smoritaemb 3:aaf6422e0be9 48 memcpy(addrPtr,&x
smoritaemb 3:aaf6422e0be9 49 ,8);
smoritaemb 3:aaf6422e0be9 50 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 51 cntPub += 8;
smoritaemb 3:aaf6422e0be9 52
smoritaemb 3:aaf6422e0be9 53
smoritaemb 3:aaf6422e0be9 54
smoritaemb 3:aaf6422e0be9 55
smoritaemb 3:aaf6422e0be9 56
smoritaemb 3:aaf6422e0be9 57 if (cntPub%8 > 0){
smoritaemb 3:aaf6422e0be9 58 for(int i=0; i<(8-(cntPub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 59 *addrPtr = 0;
smoritaemb 3:aaf6422e0be9 60 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 61 }
smoritaemb 3:aaf6422e0be9 62 cntPub += 8-(cntPub%8);
smoritaemb 3:aaf6422e0be9 63 }
smoritaemb 3:aaf6422e0be9 64
smoritaemb 3:aaf6422e0be9 65 memcpy(addrPtr,&y
smoritaemb 3:aaf6422e0be9 66 ,8);
smoritaemb 3:aaf6422e0be9 67 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 68 cntPub += 8;
smoritaemb 3:aaf6422e0be9 69
smoritaemb 3:aaf6422e0be9 70
smoritaemb 3:aaf6422e0be9 71
smoritaemb 3:aaf6422e0be9 72
smoritaemb 3:aaf6422e0be9 73
smoritaemb 3:aaf6422e0be9 74 if (cntPub%8 > 0){
smoritaemb 3:aaf6422e0be9 75 for(int i=0; i<(8-(cntPub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 76 *addrPtr = 0;
smoritaemb 3:aaf6422e0be9 77 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 78 }
smoritaemb 3:aaf6422e0be9 79 cntPub += 8-(cntPub%8);
smoritaemb 3:aaf6422e0be9 80 }
smoritaemb 3:aaf6422e0be9 81
smoritaemb 3:aaf6422e0be9 82 memcpy(addrPtr,&z
smoritaemb 3:aaf6422e0be9 83 ,8);
smoritaemb 3:aaf6422e0be9 84 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 85 cntPub += 8;
smoritaemb 3:aaf6422e0be9 86
smoritaemb 3:aaf6422e0be9 87
smoritaemb 3:aaf6422e0be9 88
smoritaemb 3:aaf6422e0be9 89
smoritaemb 3:aaf6422e0be9 90
smoritaemb 3:aaf6422e0be9 91 if (cntPub%8 > 0){
smoritaemb 3:aaf6422e0be9 92 for(int i=0; i<(8-(cntPub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 93 *addrPtr = 0;
smoritaemb 3:aaf6422e0be9 94 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 95 }
smoritaemb 3:aaf6422e0be9 96 cntPub += 8-(cntPub%8);
smoritaemb 3:aaf6422e0be9 97 }
smoritaemb 3:aaf6422e0be9 98
smoritaemb 3:aaf6422e0be9 99 memcpy(addrPtr,&w,8);
smoritaemb 3:aaf6422e0be9 100 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 101 cntPub += 8;
smoritaemb 3:aaf6422e0be9 102
smoritaemb 3:aaf6422e0be9 103
smoritaemb 3:aaf6422e0be9 104
smoritaemb 3:aaf6422e0be9 105
smoritaemb 3:aaf6422e0be9 106 return cntPub;
smoritaemb 3:aaf6422e0be9 107 }
smoritaemb 3:aaf6422e0be9 108
smoritaemb 3:aaf6422e0be9 109 uint32_t copyFromBuf(const uint8_t *addrPtr) {
smoritaemb 3:aaf6422e0be9 110 uint32_t tmpSub = 0;
smoritaemb 3:aaf6422e0be9 111 uint32_t arraySize;
smoritaemb 3:aaf6422e0be9 112 uint32_t stringSize;
smoritaemb 3:aaf6422e0be9 113
smoritaemb 3:aaf6422e0be9 114
smoritaemb 3:aaf6422e0be9 115
smoritaemb 3:aaf6422e0be9 116
smoritaemb 3:aaf6422e0be9 117 if (cntSub%8 > 0){
smoritaemb 3:aaf6422e0be9 118 for(int i=0; i<(8-(cntSub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 119 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 120 }
smoritaemb 3:aaf6422e0be9 121 cntSub += 8-(cntSub%8);
smoritaemb 3:aaf6422e0be9 122 }
smoritaemb 3:aaf6422e0be9 123
smoritaemb 3:aaf6422e0be9 124 memcpy(&x
smoritaemb 3:aaf6422e0be9 125 ,addrPtr,8);
smoritaemb 3:aaf6422e0be9 126 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 127 cntSub += 8;
smoritaemb 3:aaf6422e0be9 128
smoritaemb 3:aaf6422e0be9 129
smoritaemb 3:aaf6422e0be9 130
smoritaemb 3:aaf6422e0be9 131
smoritaemb 3:aaf6422e0be9 132 if (cntSub%8 > 0){
smoritaemb 3:aaf6422e0be9 133 for(int i=0; i<(8-(cntSub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 134 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 135 }
smoritaemb 3:aaf6422e0be9 136 cntSub += 8-(cntSub%8);
smoritaemb 3:aaf6422e0be9 137 }
smoritaemb 3:aaf6422e0be9 138
smoritaemb 3:aaf6422e0be9 139 memcpy(&y
smoritaemb 3:aaf6422e0be9 140 ,addrPtr,8);
smoritaemb 3:aaf6422e0be9 141 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 142 cntSub += 8;
smoritaemb 3:aaf6422e0be9 143
smoritaemb 3:aaf6422e0be9 144
smoritaemb 3:aaf6422e0be9 145
smoritaemb 3:aaf6422e0be9 146
smoritaemb 3:aaf6422e0be9 147 if (cntSub%8 > 0){
smoritaemb 3:aaf6422e0be9 148 for(int i=0; i<(8-(cntSub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 149 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 150 }
smoritaemb 3:aaf6422e0be9 151 cntSub += 8-(cntSub%8);
smoritaemb 3:aaf6422e0be9 152 }
smoritaemb 3:aaf6422e0be9 153
smoritaemb 3:aaf6422e0be9 154 memcpy(&z
smoritaemb 3:aaf6422e0be9 155 ,addrPtr,8);
smoritaemb 3:aaf6422e0be9 156 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 157 cntSub += 8;
smoritaemb 3:aaf6422e0be9 158
smoritaemb 3:aaf6422e0be9 159
smoritaemb 3:aaf6422e0be9 160
smoritaemb 3:aaf6422e0be9 161
smoritaemb 3:aaf6422e0be9 162 if (cntSub%8 > 0){
smoritaemb 3:aaf6422e0be9 163 for(int i=0; i<(8-(cntSub%8)) ; i++){
smoritaemb 3:aaf6422e0be9 164 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 165 }
smoritaemb 3:aaf6422e0be9 166 cntSub += 8-(cntSub%8);
smoritaemb 3:aaf6422e0be9 167 }
smoritaemb 3:aaf6422e0be9 168
smoritaemb 3:aaf6422e0be9 169 memcpy(&w,addrPtr,8);
smoritaemb 3:aaf6422e0be9 170 addrPtr += 8;
smoritaemb 3:aaf6422e0be9 171 cntSub += 8;
smoritaemb 3:aaf6422e0be9 172
smoritaemb 3:aaf6422e0be9 173
smoritaemb 3:aaf6422e0be9 174
smoritaemb 3:aaf6422e0be9 175 return cntSub;
smoritaemb 3:aaf6422e0be9 176 }
smoritaemb 3:aaf6422e0be9 177
smoritaemb 3:aaf6422e0be9 178 void memAlign(uint8_t *addrPtr){
smoritaemb 3:aaf6422e0be9 179 if (cntPub%4 > 0){
smoritaemb 3:aaf6422e0be9 180 addrPtr += cntPub;
smoritaemb 3:aaf6422e0be9 181 for(int i=0; i<(4-(cntPub%4)) ; i++){
smoritaemb 3:aaf6422e0be9 182 *addrPtr = 0;
smoritaemb 3:aaf6422e0be9 183 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 184 }
smoritaemb 3:aaf6422e0be9 185 cntPub += 4-(cntPub%4);
smoritaemb 3:aaf6422e0be9 186 }
smoritaemb 3:aaf6422e0be9 187 return;
smoritaemb 3:aaf6422e0be9 188 }
smoritaemb 3:aaf6422e0be9 189
smoritaemb 3:aaf6422e0be9 190 uint32_t getTotalSize(){
smoritaemb 3:aaf6422e0be9 191 uint32_t tmpCntPub = cntPub;
smoritaemb 3:aaf6422e0be9 192 cntPub = 0;
smoritaemb 3:aaf6422e0be9 193 return tmpCntPub ;
smoritaemb 3:aaf6422e0be9 194 }
smoritaemb 3:aaf6422e0be9 195
smoritaemb 3:aaf6422e0be9 196 private:
smoritaemb 3:aaf6422e0be9 197 std::string type_name = "geometry_msgs::msg::dds_::Quaternion";
smoritaemb 3:aaf6422e0be9 198 };
smoritaemb 3:aaf6422e0be9 199 };
smoritaemb 3:aaf6422e0be9 200 }
smoritaemb 3:aaf6422e0be9 201
smoritaemb 3:aaf6422e0be9 202 namespace message_traits
smoritaemb 3:aaf6422e0be9 203 {
smoritaemb 3:aaf6422e0be9 204 template<>
smoritaemb 3:aaf6422e0be9 205 struct TypeName<geometry_msgs::msg::Quaternion*> {
smoritaemb 3:aaf6422e0be9 206 static const char* value()
smoritaemb 3:aaf6422e0be9 207 {
smoritaemb 3:aaf6422e0be9 208 return "geometry_msgs::msg::dds_::Quaternion_";
smoritaemb 3:aaf6422e0be9 209 }
smoritaemb 3:aaf6422e0be9 210 };
smoritaemb 3:aaf6422e0be9 211 }
smoritaemb 3:aaf6422e0be9 212
smoritaemb 3:aaf6422e0be9 213 #endif