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

mros2_msgs/std_msgs/msg/header.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 #include <string>
smoritaemb 3:aaf6422e0be9 2
smoritaemb 3:aaf6422e0be9 3 namespace std_msgs
smoritaemb 3:aaf6422e0be9 4 {
smoritaemb 3:aaf6422e0be9 5 namespace msg
smoritaemb 3:aaf6422e0be9 6 {
smoritaemb 3:aaf6422e0be9 7 class Header
smoritaemb 3:aaf6422e0be9 8 {
smoritaemb 3:aaf6422e0be9 9 public:
smoritaemb 3:aaf6422e0be9 10 std::string getTypeName();
smoritaemb 3:aaf6422e0be9 11 int32_t sec;
smoritaemb 3:aaf6422e0be9 12 uint32_t nanosec;
smoritaemb 3:aaf6422e0be9 13 std::string frame_id;
smoritaemb 3:aaf6422e0be9 14 uint8_t cntPub = 0;
smoritaemb 3:aaf6422e0be9 15 uint32_t pubSize;
smoritaemb 3:aaf6422e0be9 16 uint32_t subSize;
smoritaemb 3:aaf6422e0be9 17
smoritaemb 3:aaf6422e0be9 18 void copyToBuf(uint8_t *addrPtr)
smoritaemb 3:aaf6422e0be9 19 {
smoritaemb 3:aaf6422e0be9 20 memcpy(addrPtr, &sec, 4);
smoritaemb 3:aaf6422e0be9 21 addrPtr += 4;
smoritaemb 3:aaf6422e0be9 22 cntPub += 4;
smoritaemb 3:aaf6422e0be9 23 memcpy(addrPtr, &nanosec, 4);
smoritaemb 3:aaf6422e0be9 24 addrPtr += 4;
smoritaemb 3:aaf6422e0be9 25 cntPub += 4;
smoritaemb 3:aaf6422e0be9 26 pubSize = frame_id.size();
smoritaemb 3:aaf6422e0be9 27 memcpy(addrPtr, &pubSize, 4);
smoritaemb 3:aaf6422e0be9 28 addrPtr += 4;
smoritaemb 3:aaf6422e0be9 29 cntPub += 4;
smoritaemb 3:aaf6422e0be9 30 memcpy(addrPtr, frame_id.c_str(),pubSize);
smoritaemb 3:aaf6422e0be9 31 addrPtr += pubSize;
smoritaemb 3:aaf6422e0be9 32 cntPub += pubSize;
smoritaemb 3:aaf6422e0be9 33 }
smoritaemb 3:aaf6422e0be9 34
smoritaemb 3:aaf6422e0be9 35 void copyFromBuf(const uint8_t *addrPtr)
smoritaemb 3:aaf6422e0be9 36 {
smoritaemb 3:aaf6422e0be9 37 memcpy(&sec, addrPtr, 4);
smoritaemb 3:aaf6422e0be9 38 addrPtr += 4;
smoritaemb 3:aaf6422e0be9 39 memcpy(&nanosec, addrPtr, 4);
smoritaemb 3:aaf6422e0be9 40 addrPtr += 4;
smoritaemb 3:aaf6422e0be9 41 memcpy(&subSize, addrPtr, 4);
smoritaemb 3:aaf6422e0be9 42 addrPtr += 4;
smoritaemb 3:aaf6422e0be9 43 frame_id.resize(subSize);
smoritaemb 3:aaf6422e0be9 44 memcpy(&frame_id[0], addrPtr, subSize);
smoritaemb 3:aaf6422e0be9 45 }
smoritaemb 3:aaf6422e0be9 46
smoritaemb 3:aaf6422e0be9 47 void memAlign(uint8_t *addrPtr){
smoritaemb 3:aaf6422e0be9 48 if (cntPub%4 > 0){
smoritaemb 3:aaf6422e0be9 49 addrPtr += cntPub;
smoritaemb 3:aaf6422e0be9 50 for(int i=0; i<(4-(cntPub%4)) ; i++){
smoritaemb 3:aaf6422e0be9 51 *addrPtr = 0;
smoritaemb 3:aaf6422e0be9 52 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 53 }
smoritaemb 3:aaf6422e0be9 54 cntPub += 4-(cntPub%4);
smoritaemb 3:aaf6422e0be9 55 }
smoritaemb 3:aaf6422e0be9 56 return;
smoritaemb 3:aaf6422e0be9 57 }
smoritaemb 3:aaf6422e0be9 58
smoritaemb 3:aaf6422e0be9 59 uint8_t getTotalSize()
smoritaemb 3:aaf6422e0be9 60 {
smoritaemb 3:aaf6422e0be9 61 return cntPub;
smoritaemb 3:aaf6422e0be9 62 }
smoritaemb 3:aaf6422e0be9 63 private:
smoritaemb 3:aaf6422e0be9 64 std::string type_name = "std_msgs::msg::dds_::Header";
smoritaemb 3:aaf6422e0be9 65 };
smoritaemb 3:aaf6422e0be9 66 }//namspace msg
smoritaemb 3:aaf6422e0be9 67 }//namespace std_msgs
smoritaemb 3:aaf6422e0be9 68
smoritaemb 3:aaf6422e0be9 69 namespace message_traits
smoritaemb 3:aaf6422e0be9 70 {
smoritaemb 3:aaf6422e0be9 71
smoritaemb 3:aaf6422e0be9 72 template<>
smoritaemb 3:aaf6422e0be9 73 struct TypeName<std_msgs::msg::Header*> {
smoritaemb 3:aaf6422e0be9 74 static const char* value()
smoritaemb 3:aaf6422e0be9 75 {
smoritaemb 3:aaf6422e0be9 76 return "std_msgs::msg::dds_::Header_";
smoritaemb 3:aaf6422e0be9 77 }
smoritaemb 3:aaf6422e0be9 78 };
smoritaemb 3:aaf6422e0be9 79
smoritaemb 3:aaf6422e0be9 80 }