root / quad2 / arduino / src / ros_lib / tf / tfMessage.h @ c1426757
History | View | Annotate | Download (1.56 KB)
1 |
#ifndef _ROS_tf_tfMessage_h
|
---|---|
2 |
#define _ROS_tf_tfMessage_h
|
3 |
|
4 |
#include <stdint.h> |
5 |
#include <string.h> |
6 |
#include <stdlib.h> |
7 |
#include "ros/msg.h" |
8 |
#include "geometry_msgs/TransformStamped.h" |
9 |
|
10 |
namespace tf |
11 |
{ |
12 |
|
13 |
class tfMessage : public ros::Msg |
14 |
{ |
15 |
public:
|
16 |
uint8_t transforms_length; |
17 |
geometry_msgs::TransformStamped st_transforms; |
18 |
geometry_msgs::TransformStamped * transforms; |
19 |
|
20 |
virtual int serialize(unsigned char *outbuffer) const |
21 |
{ |
22 |
int offset = 0; |
23 |
*(outbuffer + offset++) = transforms_length; |
24 |
*(outbuffer + offset++) = 0;
|
25 |
*(outbuffer + offset++) = 0;
|
26 |
*(outbuffer + offset++) = 0;
|
27 |
for( uint8_t i = 0; i < transforms_length; i++){ |
28 |
offset += this->transforms[i].serialize(outbuffer + offset); |
29 |
} |
30 |
return offset;
|
31 |
} |
32 |
|
33 |
virtual int deserialize(unsigned char *inbuffer) |
34 |
{ |
35 |
int offset = 0; |
36 |
uint8_t transforms_lengthT = *(inbuffer + offset++); |
37 |
if(transforms_lengthT > transforms_length)
|
38 |
this->transforms = (geometry_msgs::TransformStamped*)realloc(this->transforms, transforms_lengthT * sizeof(geometry_msgs::TransformStamped));
|
39 |
offset += 3;
|
40 |
transforms_length = transforms_lengthT; |
41 |
for( uint8_t i = 0; i < transforms_length; i++){ |
42 |
offset += this->st_transforms.deserialize(inbuffer + offset); |
43 |
memcpy( &(this->transforms[i]), &(this->st_transforms), sizeof(geometry_msgs::TransformStamped));
|
44 |
} |
45 |
return offset;
|
46 |
} |
47 |
|
48 |
const char * getType(){ return "tf/tfMessage"; }; |
49 |
const char * getMD5(){ return "94810edda583a504dfda3829e70d7eec"; }; |
50 |
|
51 |
}; |
52 |
|
53 |
} |
54 |
#endif
|