Statistics
| Branch: | Revision:

root / quad2 / arduino / src / ros_lib / sensor_msgs / CompressedImage.h @ c1426757

History | View | Annotate | Download (2.2 KB)

1
#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