Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (10.3 KB)

1
#ifndef _ROS_sensor_msgs_CameraInfo_h
2
#define _ROS_sensor_msgs_CameraInfo_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
#include "sensor_msgs/RegionOfInterest.h"
10

    
11
namespace sensor_msgs
12
{
13

    
14
  class CameraInfo : public ros::Msg
15
  {
16
    public:
17
      std_msgs::Header header;
18
      uint32_t height;
19
      uint32_t width;
20
      char * distortion_model;
21
      uint8_t D_length;
22
      float st_D;
23
      float * D;
24
      float K[9];
25
      float R[9];
26
      float P[12];
27
      uint32_t binning_x;
28
      uint32_t binning_y;
29
      sensor_msgs::RegionOfInterest roi;
30

    
31
    virtual int serialize(unsigned char *outbuffer) const
32
    {
33
      int offset = 0;
34
      offset += this->header.serialize(outbuffer + offset);
35
      *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF;
36
      *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF;
37
      *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF;
38
      *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF;
39
      offset += sizeof(this->height);
40
      *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF;
41
      *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF;
42
      *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF;
43
      *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF;
44
      offset += sizeof(this->width);
45
      uint32_t * length_distortion_model = (uint32_t *)(outbuffer + offset);
46
      *length_distortion_model = strlen( (const char*) this->distortion_model);
47
      offset += 4;
48
      memcpy(outbuffer + offset, this->distortion_model, *length_distortion_model);
49
      offset += *length_distortion_model;
50
      *(outbuffer + offset++) = D_length;
51
      *(outbuffer + offset++) = 0;
52
      *(outbuffer + offset++) = 0;
53
      *(outbuffer + offset++) = 0;
54
      for( uint8_t i = 0; i < D_length; i++){
55
      int32_t * val_Di = (long *) &(this->D[i]);
56
      int32_t exp_Di = (((*val_Di)>>23)&255);
57
      if(exp_Di != 0)
58
        exp_Di += 1023-127;
59
      int32_t sig_Di = *val_Di;
60
      *(outbuffer + offset++) = 0;
61
      *(outbuffer + offset++) = 0;
62
      *(outbuffer + offset++) = 0;
63
      *(outbuffer + offset++) = (sig_Di<<5) & 0xff;
64
      *(outbuffer + offset++) = (sig_Di>>3) & 0xff;
65
      *(outbuffer + offset++) = (sig_Di>>11) & 0xff;
66
      *(outbuffer + offset++) = ((exp_Di<<4) & 0xF0) | ((sig_Di>>19)&0x0F);
67
      *(outbuffer + offset++) = (exp_Di>>4) & 0x7F;
68
      if(this->D[i] < 0) *(outbuffer + offset -1) |= 0x80;
69
      }
70
      unsigned char * K_val = (unsigned char *) this->K;
71
      for( uint8_t i = 0; i < 9; i++){
72
      int32_t * val_Ki = (long *) &(this->K[i]);
73
      int32_t exp_Ki = (((*val_Ki)>>23)&255);
74
      if(exp_Ki != 0)
75
        exp_Ki += 1023-127;
76
      int32_t sig_Ki = *val_Ki;
77
      *(outbuffer + offset++) = 0;
78
      *(outbuffer + offset++) = 0;
79
      *(outbuffer + offset++) = 0;
80
      *(outbuffer + offset++) = (sig_Ki<<5) & 0xff;
81
      *(outbuffer + offset++) = (sig_Ki>>3) & 0xff;
82
      *(outbuffer + offset++) = (sig_Ki>>11) & 0xff;
83
      *(outbuffer + offset++) = ((exp_Ki<<4) & 0xF0) | ((sig_Ki>>19)&0x0F);
84
      *(outbuffer + offset++) = (exp_Ki>>4) & 0x7F;
85
      if(this->K[i] < 0) *(outbuffer + offset -1) |= 0x80;
86
      }
87
      unsigned char * R_val = (unsigned char *) this->R;
88
      for( uint8_t i = 0; i < 9; i++){
89
      int32_t * val_Ri = (long *) &(this->R[i]);
90
      int32_t exp_Ri = (((*val_Ri)>>23)&255);
91
      if(exp_Ri != 0)
92
        exp_Ri += 1023-127;
93
      int32_t sig_Ri = *val_Ri;
94
      *(outbuffer + offset++) = 0;
95
      *(outbuffer + offset++) = 0;
96
      *(outbuffer + offset++) = 0;
97
      *(outbuffer + offset++) = (sig_Ri<<5) & 0xff;
98
      *(outbuffer + offset++) = (sig_Ri>>3) & 0xff;
99
      *(outbuffer + offset++) = (sig_Ri>>11) & 0xff;
100
      *(outbuffer + offset++) = ((exp_Ri<<4) & 0xF0) | ((sig_Ri>>19)&0x0F);
101
      *(outbuffer + offset++) = (exp_Ri>>4) & 0x7F;
102
      if(this->R[i] < 0) *(outbuffer + offset -1) |= 0x80;
103
      }
104
      unsigned char * P_val = (unsigned char *) this->P;
105
      for( uint8_t i = 0; i < 12; i++){
106
      int32_t * val_Pi = (long *) &(this->P[i]);
107
      int32_t exp_Pi = (((*val_Pi)>>23)&255);
108
      if(exp_Pi != 0)
109
        exp_Pi += 1023-127;
110
      int32_t sig_Pi = *val_Pi;
111
      *(outbuffer + offset++) = 0;
112
      *(outbuffer + offset++) = 0;
113
      *(outbuffer + offset++) = 0;
114
      *(outbuffer + offset++) = (sig_Pi<<5) & 0xff;
115
      *(outbuffer + offset++) = (sig_Pi>>3) & 0xff;
116
      *(outbuffer + offset++) = (sig_Pi>>11) & 0xff;
117
      *(outbuffer + offset++) = ((exp_Pi<<4) & 0xF0) | ((sig_Pi>>19)&0x0F);
118
      *(outbuffer + offset++) = (exp_Pi>>4) & 0x7F;
119
      if(this->P[i] < 0) *(outbuffer + offset -1) |= 0x80;
120
      }
121
      *(outbuffer + offset + 0) = (this->binning_x >> (8 * 0)) & 0xFF;
122
      *(outbuffer + offset + 1) = (this->binning_x >> (8 * 1)) & 0xFF;
123
      *(outbuffer + offset + 2) = (this->binning_x >> (8 * 2)) & 0xFF;
124
      *(outbuffer + offset + 3) = (this->binning_x >> (8 * 3)) & 0xFF;
125
      offset += sizeof(this->binning_x);
126
      *(outbuffer + offset + 0) = (this->binning_y >> (8 * 0)) & 0xFF;
127
      *(outbuffer + offset + 1) = (this->binning_y >> (8 * 1)) & 0xFF;
128
      *(outbuffer + offset + 2) = (this->binning_y >> (8 * 2)) & 0xFF;
129
      *(outbuffer + offset + 3) = (this->binning_y >> (8 * 3)) & 0xFF;
130
      offset += sizeof(this->binning_y);
131
      offset += this->roi.serialize(outbuffer + offset);
132
      return offset;
133
    }
134

    
135
    virtual int deserialize(unsigned char *inbuffer)
136
    {
137
      int offset = 0;
138
      offset += this->header.deserialize(inbuffer + offset);
139
      this->height |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
140
      this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
141
      this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
142
      this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
143
      offset += sizeof(this->height);
144
      this->width |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
145
      this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
146
      this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
147
      this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
148
      offset += sizeof(this->width);
149
      uint32_t length_distortion_model = *(uint32_t *)(inbuffer + offset);
150
      offset += 4;
151
      for(unsigned int k= offset; k< offset+length_distortion_model; ++k){
152
          inbuffer[k-1]=inbuffer[k];
153
      }
154
      inbuffer[offset+length_distortion_model-1]=0;
155
      this->distortion_model = (char *)(inbuffer + offset-1);
156
      offset += length_distortion_model;
157
      uint8_t D_lengthT = *(inbuffer + offset++);
158
      if(D_lengthT > D_length)
159
        this->D = (float*)realloc(this->D, D_lengthT * sizeof(float));
160
      offset += 3;
161
      D_length = D_lengthT;
162
      for( uint8_t i = 0; i < D_length; i++){
163
      uint32_t * val_st_D = (uint32_t*) &(this->st_D);
164
      offset += 3;
165
      *val_st_D = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
166
      *val_st_D |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
167
      *val_st_D |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
168
      *val_st_D |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
169
      uint32_t exp_st_D = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
170
      exp_st_D |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
171
      if(exp_st_D !=0)
172
        *val_st_D |= ((exp_st_D)-1023+127)<<23;
173
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_D = -this->st_D;
174
        memcpy( &(this->D[i]), &(this->st_D), sizeof(float));
175
      }
176
      uint8_t * K_val = (uint8_t*) this->K;
177
      for( uint8_t i = 0; i < 9; i++){
178
      uint32_t * val_Ki = (uint32_t*) &(this->K[i]);
179
      offset += 3;
180
      *val_Ki = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
181
      *val_Ki |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
182
      *val_Ki |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
183
      *val_Ki |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
184
      uint32_t exp_Ki = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
185
      exp_Ki |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
186
      if(exp_Ki !=0)
187
        *val_Ki |= ((exp_Ki)-1023+127)<<23;
188
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->K[i] = -this->K[i];
189
      }
190
      uint8_t * R_val = (uint8_t*) this->R;
191
      for( uint8_t i = 0; i < 9; i++){
192
      uint32_t * val_Ri = (uint32_t*) &(this->R[i]);
193
      offset += 3;
194
      *val_Ri = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
195
      *val_Ri |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
196
      *val_Ri |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
197
      *val_Ri |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
198
      uint32_t exp_Ri = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
199
      exp_Ri |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
200
      if(exp_Ri !=0)
201
        *val_Ri |= ((exp_Ri)-1023+127)<<23;
202
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->R[i] = -this->R[i];
203
      }
204
      uint8_t * P_val = (uint8_t*) this->P;
205
      for( uint8_t i = 0; i < 12; i++){
206
      uint32_t * val_Pi = (uint32_t*) &(this->P[i]);
207
      offset += 3;
208
      *val_Pi = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
209
      *val_Pi |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
210
      *val_Pi |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
211
      *val_Pi |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
212
      uint32_t exp_Pi = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
213
      exp_Pi |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
214
      if(exp_Pi !=0)
215
        *val_Pi |= ((exp_Pi)-1023+127)<<23;
216
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->P[i] = -this->P[i];
217
      }
218
      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
219
      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
220
      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
221
      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
222
      offset += sizeof(this->binning_x);
223
      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
224
      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
225
      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
226
      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
227
      offset += sizeof(this->binning_y);
228
      offset += this->roi.deserialize(inbuffer + offset);
229
     return offset;
230
    }
231

    
232
    const char * getType(){ return "sensor_msgs/CameraInfo"; };
233
    const char * getMD5(){ return "c9a58c1b0b154e0e6da7578cb991d214"; };
234

    
235
  };
236

    
237
}
238
#endif