Statistics
| Branch: | Revision:

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