root / quad2 / arduino / src / ros_lib / sensor_msgs / CompressedImage.h @ c1426757
History | View | Annotate | Download (2.2 KB)
1 | c1426757 | Tom Mullins | #ifndef _ROS_sensor_msgs_CompressedImage_h
|
---|---|---|---|
2 | #define _ROS_sensor_msgs_CompressedImage_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 CompressedImage : public ros::Msg |
||
14 | { |
||
15 | public:
|
||
16 | std_msgs::Header header; |
||
17 | char * format;
|
||
18 | uint8_t data_length; |
||
19 | uint8_t st_data; |
||
20 | uint8_t * data; |
||
21 | |||
22 | virtual int serialize(unsigned char *outbuffer) const |
||
23 | { |
||
24 | int offset = 0; |
||
25 | offset += this->header.serialize(outbuffer + offset); |
||
26 | uint32_t * length_format = (uint32_t *)(outbuffer + offset); |
||
27 | *length_format = strlen( (const char*) this->format); |
||
28 | offset += 4;
|
||
29 | memcpy(outbuffer + offset, this->format, *length_format); |
||
30 | offset += *length_format; |
||
31 | *(outbuffer + offset++) = data_length; |
||
32 | *(outbuffer + offset++) = 0;
|
||
33 | *(outbuffer + offset++) = 0;
|
||
34 | *(outbuffer + offset++) = 0;
|
||
35 | for( uint8_t i = 0; i < data_length; i++){ |
||
36 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; |
||
37 | offset += sizeof(this->data[i]);
|
||
38 | } |
||
39 | return offset;
|
||
40 | } |
||
41 | |||
42 | virtual int deserialize(unsigned char *inbuffer) |
||
43 | { |
||
44 | int offset = 0; |
||
45 | offset += this->header.deserialize(inbuffer + offset); |
||
46 | uint32_t length_format = *(uint32_t *)(inbuffer + offset); |
||
47 | offset += 4;
|
||
48 | for(unsigned int k= offset; k< offset+length_format; ++k){ |
||
49 | inbuffer[k-1]=inbuffer[k];
|
||
50 | } |
||
51 | inbuffer[offset+length_format-1]=0; |
||
52 | this->format = (char *)(inbuffer + offset-1); |
||
53 | offset += length_format; |
||
54 | uint8_t data_lengthT = *(inbuffer + offset++); |
||
55 | if(data_lengthT > data_length)
|
||
56 | this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t));
|
||
57 | offset += 3;
|
||
58 | data_length = data_lengthT; |
||
59 | for( uint8_t i = 0; i < data_length; i++){ |
||
60 | this->st_data |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
61 | offset += sizeof(this->st_data);
|
||
62 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t));
|
||
63 | } |
||
64 | return offset;
|
||
65 | } |
||
66 | |||
67 | const char * getType(){ return "sensor_msgs/CompressedImage"; }; |
||
68 | const char * getMD5(){ return "8f7a12909da2c9d3332d540a0977563f"; }; |
||
69 | |||
70 | }; |
||
71 | |||
72 | } |
||
73 | #endif |