root / quad2 / arduino / src / ros_lib / sensor_msgs / Image.h @ c1426757
History | View | Annotate | Download (4.37 KB)
1 |
#ifndef _ROS_sensor_msgs_Image_h
|
---|---|
2 |
#define _ROS_sensor_msgs_Image_h
|
3 |
|
4 |
#include <stdint.h> |
5 |
#include <string.h> |
6 |
#include <stdlib.h> |
7 |
#include "ros/msg.h" |
8 |
#include "std_msgs/Header.h" |
9 |
|
10 |
namespace sensor_msgs |
11 |
{ |
12 |
|
13 |
class Image : public ros::Msg |
14 |
{ |
15 |
public:
|
16 |
std_msgs::Header header; |
17 |
uint32_t height; |
18 |
uint32_t width; |
19 |
char * encoding;
|
20 |
uint8_t is_bigendian; |
21 |
uint32_t step; |
22 |
uint8_t data_length; |
23 |
uint8_t st_data; |
24 |
uint8_t * data; |
25 |
|
26 |
virtual int serialize(unsigned char *outbuffer) const |
27 |
{ |
28 |
int offset = 0; |
29 |
offset += this->header.serialize(outbuffer + offset); |
30 |
*(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF; |
31 |
*(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF; |
32 |
*(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF; |
33 |
*(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF; |
34 |
offset += sizeof(this->height);
|
35 |
*(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF; |
36 |
*(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF; |
37 |
*(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF; |
38 |
*(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF; |
39 |
offset += sizeof(this->width);
|
40 |
uint32_t * length_encoding = (uint32_t *)(outbuffer + offset); |
41 |
*length_encoding = strlen( (const char*) this->encoding); |
42 |
offset += 4;
|
43 |
memcpy(outbuffer + offset, this->encoding, *length_encoding); |
44 |
offset += *length_encoding; |
45 |
*(outbuffer + offset + 0) = (this->is_bigendian >> (8 * 0)) & 0xFF; |
46 |
offset += sizeof(this->is_bigendian);
|
47 |
*(outbuffer + offset + 0) = (this->step >> (8 * 0)) & 0xFF; |
48 |
*(outbuffer + offset + 1) = (this->step >> (8 * 1)) & 0xFF; |
49 |
*(outbuffer + offset + 2) = (this->step >> (8 * 2)) & 0xFF; |
50 |
*(outbuffer + offset + 3) = (this->step >> (8 * 3)) & 0xFF; |
51 |
offset += sizeof(this->step);
|
52 |
*(outbuffer + offset++) = data_length; |
53 |
*(outbuffer + offset++) = 0;
|
54 |
*(outbuffer + offset++) = 0;
|
55 |
*(outbuffer + offset++) = 0;
|
56 |
for( uint8_t i = 0; i < data_length; i++){ |
57 |
*(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; |
58 |
offset += sizeof(this->data[i]);
|
59 |
} |
60 |
return offset;
|
61 |
} |
62 |
|
63 |
virtual int deserialize(unsigned char *inbuffer) |
64 |
{ |
65 |
int offset = 0; |
66 |
offset += this->header.deserialize(inbuffer + offset); |
67 |
this->height |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
68 |
this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
69 |
this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
70 |
this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
71 |
offset += sizeof(this->height);
|
72 |
this->width |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
73 |
this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
74 |
this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
75 |
this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
76 |
offset += sizeof(this->width);
|
77 |
uint32_t length_encoding = *(uint32_t *)(inbuffer + offset); |
78 |
offset += 4;
|
79 |
for(unsigned int k= offset; k< offset+length_encoding; ++k){ |
80 |
inbuffer[k-1]=inbuffer[k];
|
81 |
} |
82 |
inbuffer[offset+length_encoding-1]=0; |
83 |
this->encoding = (char *)(inbuffer + offset-1); |
84 |
offset += length_encoding; |
85 |
this->is_bigendian |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); |
86 |
offset += sizeof(this->is_bigendian);
|
87 |
this->step |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
88 |
this->step |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
89 |
this->step |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
90 |
this->step |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
91 |
offset += sizeof(this->step);
|
92 |
uint8_t data_lengthT = *(inbuffer + offset++); |
93 |
if(data_lengthT > data_length)
|
94 |
this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t));
|
95 |
offset += 3;
|
96 |
data_length = data_lengthT; |
97 |
for( uint8_t i = 0; i < data_length; i++){ |
98 |
this->st_data |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); |
99 |
offset += sizeof(this->st_data);
|
100 |
memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t));
|
101 |
} |
102 |
return offset;
|
103 |
} |
104 |
|
105 |
const char * getType(){ return "sensor_msgs/Image"; }; |
106 |
const char * getMD5(){ return "060021388200f6f0f447d0fcd9c64743"; }; |
107 |
|
108 |
}; |
109 |
|
110 |
} |
111 |
#endif
|