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/pose.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_POSE_H
smoritaemb 3:aaf6422e0be9 2 #define _GEOMETRY_MSGS_MSG_POSE_H
smoritaemb 3:aaf6422e0be9 3
smoritaemb 3:aaf6422e0be9 4 #include <iostream>
smoritaemb 3:aaf6422e0be9 5 #include <string>
smoritaemb 3:aaf6422e0be9 6 #include "geometry_msgs/msg/point.hpp"
smoritaemb 3:aaf6422e0be9 7 #include "geometry_msgs/msg/quaternion.hpp"
smoritaemb 3:aaf6422e0be9 8
smoritaemb 3:aaf6422e0be9 9 using namespace std;
smoritaemb 3:aaf6422e0be9 10
smoritaemb 3:aaf6422e0be9 11 namespace geometry_msgs
smoritaemb 3:aaf6422e0be9 12 {
smoritaemb 3:aaf6422e0be9 13 namespace msg
smoritaemb 3:aaf6422e0be9 14 {
smoritaemb 3:aaf6422e0be9 15 class Pose
smoritaemb 3:aaf6422e0be9 16 {
smoritaemb 3:aaf6422e0be9 17 public:
smoritaemb 3:aaf6422e0be9 18 uint32_t cntPub = 0;
smoritaemb 3:aaf6422e0be9 19 uint32_t cntSub = 0;
smoritaemb 3:aaf6422e0be9 20
smoritaemb 3:aaf6422e0be9 21
smoritaemb 3:aaf6422e0be9 22 geometry_msgs::msg::Point position
smoritaemb 3:aaf6422e0be9 23 ;
smoritaemb 3:aaf6422e0be9 24
smoritaemb 3:aaf6422e0be9 25 geometry_msgs::msg::Quaternion orientation;
smoritaemb 3:aaf6422e0be9 26
smoritaemb 3:aaf6422e0be9 27
smoritaemb 3:aaf6422e0be9 28 uint32_t copyToBuf(uint8_t *addrPtr)
smoritaemb 3:aaf6422e0be9 29 {
smoritaemb 3:aaf6422e0be9 30 uint32_t tmpPub = 0;
smoritaemb 3:aaf6422e0be9 31 uint32_t arraySize;
smoritaemb 3:aaf6422e0be9 32 uint32_t stringSize;
smoritaemb 3:aaf6422e0be9 33
smoritaemb 3:aaf6422e0be9 34
smoritaemb 3:aaf6422e0be9 35
smoritaemb 3:aaf6422e0be9 36 tmpPub = position
smoritaemb 3:aaf6422e0be9 37 .copyToBuf(addrPtr);
smoritaemb 3:aaf6422e0be9 38 cntPub += tmpPub;
smoritaemb 3:aaf6422e0be9 39 addrPtr += tmpPub;
smoritaemb 3:aaf6422e0be9 40
smoritaemb 3:aaf6422e0be9 41
smoritaemb 3:aaf6422e0be9 42
smoritaemb 3:aaf6422e0be9 43
smoritaemb 3:aaf6422e0be9 44
smoritaemb 3:aaf6422e0be9 45 tmpPub = orientation.copyToBuf(addrPtr);
smoritaemb 3:aaf6422e0be9 46 cntPub += tmpPub;
smoritaemb 3:aaf6422e0be9 47 addrPtr += tmpPub;
smoritaemb 3:aaf6422e0be9 48
smoritaemb 3:aaf6422e0be9 49
smoritaemb 3:aaf6422e0be9 50
smoritaemb 3:aaf6422e0be9 51
smoritaemb 3:aaf6422e0be9 52 return cntPub;
smoritaemb 3:aaf6422e0be9 53 }
smoritaemb 3:aaf6422e0be9 54
smoritaemb 3:aaf6422e0be9 55 uint32_t copyFromBuf(const uint8_t *addrPtr) {
smoritaemb 3:aaf6422e0be9 56 uint32_t tmpSub = 0;
smoritaemb 3:aaf6422e0be9 57 uint32_t arraySize;
smoritaemb 3:aaf6422e0be9 58 uint32_t stringSize;
smoritaemb 3:aaf6422e0be9 59
smoritaemb 3:aaf6422e0be9 60
smoritaemb 3:aaf6422e0be9 61
smoritaemb 3:aaf6422e0be9 62
smoritaemb 3:aaf6422e0be9 63 tmpSub = position
smoritaemb 3:aaf6422e0be9 64 .copyFromBuf(addrPtr);
smoritaemb 3:aaf6422e0be9 65 cntSub += tmpSub;
smoritaemb 3:aaf6422e0be9 66 addrPtr += tmpSub;
smoritaemb 3:aaf6422e0be9 67
smoritaemb 3:aaf6422e0be9 68
smoritaemb 3:aaf6422e0be9 69
smoritaemb 3:aaf6422e0be9 70
smoritaemb 3:aaf6422e0be9 71
smoritaemb 3:aaf6422e0be9 72
smoritaemb 3:aaf6422e0be9 73 tmpSub = orientation.copyFromBuf(addrPtr);
smoritaemb 3:aaf6422e0be9 74 cntSub += tmpSub;
smoritaemb 3:aaf6422e0be9 75 addrPtr += tmpSub;
smoritaemb 3:aaf6422e0be9 76
smoritaemb 3:aaf6422e0be9 77
smoritaemb 3:aaf6422e0be9 78
smoritaemb 3:aaf6422e0be9 79
smoritaemb 3:aaf6422e0be9 80
smoritaemb 3:aaf6422e0be9 81 return cntSub;
smoritaemb 3:aaf6422e0be9 82 }
smoritaemb 3:aaf6422e0be9 83
smoritaemb 3:aaf6422e0be9 84 void memAlign(uint8_t *addrPtr){
smoritaemb 3:aaf6422e0be9 85 if (cntPub%4 > 0){
smoritaemb 3:aaf6422e0be9 86 addrPtr += cntPub;
smoritaemb 3:aaf6422e0be9 87 for(int i=0; i<(4-(cntPub%4)) ; i++){
smoritaemb 3:aaf6422e0be9 88 *addrPtr = 0;
smoritaemb 3:aaf6422e0be9 89 addrPtr += 1;
smoritaemb 3:aaf6422e0be9 90 }
smoritaemb 3:aaf6422e0be9 91 cntPub += 4-(cntPub%4);
smoritaemb 3:aaf6422e0be9 92 }
smoritaemb 3:aaf6422e0be9 93 return;
smoritaemb 3:aaf6422e0be9 94 }
smoritaemb 3:aaf6422e0be9 95
smoritaemb 3:aaf6422e0be9 96 uint32_t getTotalSize(){
smoritaemb 3:aaf6422e0be9 97 uint32_t tmpCntPub = cntPub;
smoritaemb 3:aaf6422e0be9 98 cntPub = 0;
smoritaemb 3:aaf6422e0be9 99 return tmpCntPub ;
smoritaemb 3:aaf6422e0be9 100 }
smoritaemb 3:aaf6422e0be9 101
smoritaemb 3:aaf6422e0be9 102 private:
smoritaemb 3:aaf6422e0be9 103 std::string type_name = "geometry_msgs::msg::dds_::Pose";
smoritaemb 3:aaf6422e0be9 104 };
smoritaemb 3:aaf6422e0be9 105 };
smoritaemb 3:aaf6422e0be9 106 }
smoritaemb 3:aaf6422e0be9 107
smoritaemb 3:aaf6422e0be9 108 namespace message_traits
smoritaemb 3:aaf6422e0be9 109 {
smoritaemb 3:aaf6422e0be9 110 template<>
smoritaemb 3:aaf6422e0be9 111 struct TypeName<geometry_msgs::msg::Pose*> {
smoritaemb 3:aaf6422e0be9 112 static const char* value()
smoritaemb 3:aaf6422e0be9 113 {
smoritaemb 3:aaf6422e0be9 114 return "geometry_msgs::msg::dds_::Pose_";
smoritaemb 3:aaf6422e0be9 115 }
smoritaemb 3:aaf6422e0be9 116 };
smoritaemb 3:aaf6422e0be9 117 }
smoritaemb 3:aaf6422e0be9 118
smoritaemb 3:aaf6422e0be9 119 #endif