Go to the documentation of this file.00001 #ifndef ros_sensor_msgs_CameraInfo_h
00002 #define ros_sensor_msgs_CameraInfo_h
00003
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "../ros/msg.h"
00008 #include "std_msgs/Header.h"
00009 #include "sensor_msgs/RegionOfInterest.h"
00010
00011 namespace sensor_msgs
00012 {
00013
00014 class CameraInfo : public ros::Msg
00015 {
00016 public:
00017 std_msgs::Header header;
00018 unsigned long height;
00019 unsigned long width;
00020 char * distortion_model;
00021 unsigned char D_length;
00022 float st_D;
00023 float * D;
00024 float K[9];
00025 float R[9];
00026 float P[12];
00027 unsigned long binning_x;
00028 unsigned long binning_y;
00029 sensor_msgs::RegionOfInterest roi;
00030
00031 virtual int serialize(unsigned char *outbuffer)
00032 {
00033 int offset = 0;
00034 offset += this->header.serialize(outbuffer + offset);
00035 union {
00036 unsigned long real;
00037 unsigned long base;
00038 } u_height;
00039 u_height.real = this->height;
00040 *(outbuffer + offset + 0) = (u_height.base >> (8 * 0)) & 0xFF;
00041 *(outbuffer + offset + 1) = (u_height.base >> (8 * 1)) & 0xFF;
00042 *(outbuffer + offset + 2) = (u_height.base >> (8 * 2)) & 0xFF;
00043 *(outbuffer + offset + 3) = (u_height.base >> (8 * 3)) & 0xFF;
00044 offset += sizeof(this->height);
00045 union {
00046 unsigned long real;
00047 unsigned long base;
00048 } u_width;
00049 u_width.real = this->width;
00050 *(outbuffer + offset + 0) = (u_width.base >> (8 * 0)) & 0xFF;
00051 *(outbuffer + offset + 1) = (u_width.base >> (8 * 1)) & 0xFF;
00052 *(outbuffer + offset + 2) = (u_width.base >> (8 * 2)) & 0xFF;
00053 *(outbuffer + offset + 3) = (u_width.base >> (8 * 3)) & 0xFF;
00054 offset += sizeof(this->width);
00055 long * length_distortion_model = (long *)(outbuffer + offset);
00056 *length_distortion_model = strlen( (const char*) this->distortion_model);
00057 offset += 4;
00058 memcpy(outbuffer + offset, this->distortion_model, *length_distortion_model);
00059 offset += *length_distortion_model;
00060 *(outbuffer + offset++) = D_length;
00061 *(outbuffer + offset++) = 0;
00062 *(outbuffer + offset++) = 0;
00063 *(outbuffer + offset++) = 0;
00064 for( unsigned char i = 0; i < D_length; i++){
00065 long * val_Di = (long *) &(this->D[i]);
00066 long exp_Di = (((*val_Di)>>23)&255);
00067 if(exp_Di != 0)
00068 exp_Di += 1023-127;
00069 long sig_Di = *val_Di;
00070 *(outbuffer + offset++) = 0;
00071 *(outbuffer + offset++) = 0;
00072 *(outbuffer + offset++) = 0;
00073 *(outbuffer + offset++) = (sig_Di<<5) & 0xff;
00074 *(outbuffer + offset++) = (sig_Di>>3) & 0xff;
00075 *(outbuffer + offset++) = (sig_Di>>11) & 0xff;
00076 *(outbuffer + offset++) = ((exp_Di<<4) & 0xF0) | ((sig_Di>>19)&0x0F);
00077 *(outbuffer + offset++) = (exp_Di>>4) & 0x7F;
00078 if(this->D[i] < 0) *(outbuffer + offset -1) |= 0x80;
00079 }
00080 unsigned char * K_val = (unsigned char *) this->K;
00081 for( unsigned char i = 0; i < 9; i++){
00082 long * val_Ki = (long *) &(this->K[i]);
00083 long exp_Ki = (((*val_Ki)>>23)&255);
00084 if(exp_Ki != 0)
00085 exp_Ki += 1023-127;
00086 long sig_Ki = *val_Ki;
00087 *(outbuffer + offset++) = 0;
00088 *(outbuffer + offset++) = 0;
00089 *(outbuffer + offset++) = 0;
00090 *(outbuffer + offset++) = (sig_Ki<<5) & 0xff;
00091 *(outbuffer + offset++) = (sig_Ki>>3) & 0xff;
00092 *(outbuffer + offset++) = (sig_Ki>>11) & 0xff;
00093 *(outbuffer + offset++) = ((exp_Ki<<4) & 0xF0) | ((sig_Ki>>19)&0x0F);
00094 *(outbuffer + offset++) = (exp_Ki>>4) & 0x7F;
00095 if(this->K[i] < 0) *(outbuffer + offset -1) |= 0x80;
00096 }
00097 unsigned char * R_val = (unsigned char *) this->R;
00098 for( unsigned char i = 0; i < 9; i++){
00099 long * val_Ri = (long *) &(this->R[i]);
00100 long exp_Ri = (((*val_Ri)>>23)&255);
00101 if(exp_Ri != 0)
00102 exp_Ri += 1023-127;
00103 long sig_Ri = *val_Ri;
00104 *(outbuffer + offset++) = 0;
00105 *(outbuffer + offset++) = 0;
00106 *(outbuffer + offset++) = 0;
00107 *(outbuffer + offset++) = (sig_Ri<<5) & 0xff;
00108 *(outbuffer + offset++) = (sig_Ri>>3) & 0xff;
00109 *(outbuffer + offset++) = (sig_Ri>>11) & 0xff;
00110 *(outbuffer + offset++) = ((exp_Ri<<4) & 0xF0) | ((sig_Ri>>19)&0x0F);
00111 *(outbuffer + offset++) = (exp_Ri>>4) & 0x7F;
00112 if(this->R[i] < 0) *(outbuffer + offset -1) |= 0x80;
00113 }
00114 unsigned char * P_val = (unsigned char *) this->P;
00115 for( unsigned char i = 0; i < 12; i++){
00116 long * val_Pi = (long *) &(this->P[i]);
00117 long exp_Pi = (((*val_Pi)>>23)&255);
00118 if(exp_Pi != 0)
00119 exp_Pi += 1023-127;
00120 long sig_Pi = *val_Pi;
00121 *(outbuffer + offset++) = 0;
00122 *(outbuffer + offset++) = 0;
00123 *(outbuffer + offset++) = 0;
00124 *(outbuffer + offset++) = (sig_Pi<<5) & 0xff;
00125 *(outbuffer + offset++) = (sig_Pi>>3) & 0xff;
00126 *(outbuffer + offset++) = (sig_Pi>>11) & 0xff;
00127 *(outbuffer + offset++) = ((exp_Pi<<4) & 0xF0) | ((sig_Pi>>19)&0x0F);
00128 *(outbuffer + offset++) = (exp_Pi>>4) & 0x7F;
00129 if(this->P[i] < 0) *(outbuffer + offset -1) |= 0x80;
00130 }
00131 union {
00132 unsigned long real;
00133 unsigned long base;
00134 } u_binning_x;
00135 u_binning_x.real = this->binning_x;
00136 *(outbuffer + offset + 0) = (u_binning_x.base >> (8 * 0)) & 0xFF;
00137 *(outbuffer + offset + 1) = (u_binning_x.base >> (8 * 1)) & 0xFF;
00138 *(outbuffer + offset + 2) = (u_binning_x.base >> (8 * 2)) & 0xFF;
00139 *(outbuffer + offset + 3) = (u_binning_x.base >> (8 * 3)) & 0xFF;
00140 offset += sizeof(this->binning_x);
00141 union {
00142 unsigned long real;
00143 unsigned long base;
00144 } u_binning_y;
00145 u_binning_y.real = this->binning_y;
00146 *(outbuffer + offset + 0) = (u_binning_y.base >> (8 * 0)) & 0xFF;
00147 *(outbuffer + offset + 1) = (u_binning_y.base >> (8 * 1)) & 0xFF;
00148 *(outbuffer + offset + 2) = (u_binning_y.base >> (8 * 2)) & 0xFF;
00149 *(outbuffer + offset + 3) = (u_binning_y.base >> (8 * 3)) & 0xFF;
00150 offset += sizeof(this->binning_y);
00151 offset += this->roi.serialize(outbuffer + offset);
00152 return offset;
00153 }
00154
00155 virtual int deserialize(unsigned char *inbuffer)
00156 {
00157 int offset = 0;
00158 offset += this->header.deserialize(inbuffer + offset);
00159 union {
00160 unsigned long real;
00161 unsigned long base;
00162 } u_height;
00163 u_height.base = 0;
00164 u_height.base |= ((typeof(u_height.base)) (*(inbuffer + offset + 0))) << (8 * 0);
00165 u_height.base |= ((typeof(u_height.base)) (*(inbuffer + offset + 1))) << (8 * 1);
00166 u_height.base |= ((typeof(u_height.base)) (*(inbuffer + offset + 2))) << (8 * 2);
00167 u_height.base |= ((typeof(u_height.base)) (*(inbuffer + offset + 3))) << (8 * 3);
00168 this->height = u_height.real;
00169 offset += sizeof(this->height);
00170 union {
00171 unsigned long real;
00172 unsigned long base;
00173 } u_width;
00174 u_width.base = 0;
00175 u_width.base |= ((typeof(u_width.base)) (*(inbuffer + offset + 0))) << (8 * 0);
00176 u_width.base |= ((typeof(u_width.base)) (*(inbuffer + offset + 1))) << (8 * 1);
00177 u_width.base |= ((typeof(u_width.base)) (*(inbuffer + offset + 2))) << (8 * 2);
00178 u_width.base |= ((typeof(u_width.base)) (*(inbuffer + offset + 3))) << (8 * 3);
00179 this->width = u_width.real;
00180 offset += sizeof(this->width);
00181 uint32_t length_distortion_model = *(uint32_t *)(inbuffer + offset);
00182 offset += 4;
00183 for(unsigned int k= offset; k< offset+length_distortion_model; ++k){
00184 inbuffer[k-1]=inbuffer[k];
00185 }
00186 inbuffer[offset+length_distortion_model-1]=0;
00187 this->distortion_model = (char *)(inbuffer + offset-1);
00188 offset += length_distortion_model;
00189 unsigned char D_lengthT = *(inbuffer + offset++);
00190 if(D_lengthT > D_length)
00191 this->D = (float*)realloc(this->D, D_lengthT * sizeof(float));
00192 offset += 3;
00193 D_length = D_lengthT;
00194 for( unsigned char i = 0; i < D_length; i++){
00195 unsigned long * val_st_D = (unsigned long*) &(this->st_D);
00196 offset += 3;
00197 *val_st_D = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
00198 *val_st_D |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
00199 *val_st_D |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
00200 *val_st_D |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
00201 unsigned long exp_st_D = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
00202 exp_st_D |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
00203 if(exp_st_D !=0)
00204 *val_st_D |= ((exp_st_D)-1023+127)<<23;
00205 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_D = -this->st_D;
00206 memcpy( &(this->D[i]), &(this->st_D), sizeof(float));
00207 }
00208 unsigned char * K_val = (unsigned char *) this->K;
00209 for( unsigned char i = 0; i < 9; i++){
00210 unsigned long * val_Ki = (unsigned long*) &(this->K[i]);
00211 offset += 3;
00212 *val_Ki = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
00213 *val_Ki |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
00214 *val_Ki |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
00215 *val_Ki |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
00216 unsigned long exp_Ki = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
00217 exp_Ki |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
00218 if(exp_Ki !=0)
00219 *val_Ki |= ((exp_Ki)-1023+127)<<23;
00220 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->K[i] = -this->K[i];
00221 }
00222 unsigned char * R_val = (unsigned char *) this->R;
00223 for( unsigned char i = 0; i < 9; i++){
00224 unsigned long * val_Ri = (unsigned long*) &(this->R[i]);
00225 offset += 3;
00226 *val_Ri = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
00227 *val_Ri |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
00228 *val_Ri |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
00229 *val_Ri |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
00230 unsigned long exp_Ri = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
00231 exp_Ri |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
00232 if(exp_Ri !=0)
00233 *val_Ri |= ((exp_Ri)-1023+127)<<23;
00234 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->R[i] = -this->R[i];
00235 }
00236 unsigned char * P_val = (unsigned char *) this->P;
00237 for( unsigned char i = 0; i < 12; i++){
00238 unsigned long * val_Pi = (unsigned long*) &(this->P[i]);
00239 offset += 3;
00240 *val_Pi = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
00241 *val_Pi |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
00242 *val_Pi |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
00243 *val_Pi |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
00244 unsigned long exp_Pi = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
00245 exp_Pi |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
00246 if(exp_Pi !=0)
00247 *val_Pi |= ((exp_Pi)-1023+127)<<23;
00248 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->P[i] = -this->P[i];
00249 }
00250 union {
00251 unsigned long real;
00252 unsigned long base;
00253 } u_binning_x;
00254 u_binning_x.base = 0;
00255 u_binning_x.base |= ((typeof(u_binning_x.base)) (*(inbuffer + offset + 0))) << (8 * 0);
00256 u_binning_x.base |= ((typeof(u_binning_x.base)) (*(inbuffer + offset + 1))) << (8 * 1);
00257 u_binning_x.base |= ((typeof(u_binning_x.base)) (*(inbuffer + offset + 2))) << (8 * 2);
00258 u_binning_x.base |= ((typeof(u_binning_x.base)) (*(inbuffer + offset + 3))) << (8 * 3);
00259 this->binning_x = u_binning_x.real;
00260 offset += sizeof(this->binning_x);
00261 union {
00262 unsigned long real;
00263 unsigned long base;
00264 } u_binning_y;
00265 u_binning_y.base = 0;
00266 u_binning_y.base |= ((typeof(u_binning_y.base)) (*(inbuffer + offset + 0))) << (8 * 0);
00267 u_binning_y.base |= ((typeof(u_binning_y.base)) (*(inbuffer + offset + 1))) << (8 * 1);
00268 u_binning_y.base |= ((typeof(u_binning_y.base)) (*(inbuffer + offset + 2))) << (8 * 2);
00269 u_binning_y.base |= ((typeof(u_binning_y.base)) (*(inbuffer + offset + 3))) << (8 * 3);
00270 this->binning_y = u_binning_y.real;
00271 offset += sizeof(this->binning_y);
00272 offset += this->roi.deserialize(inbuffer + offset);
00273 return offset;
00274 }
00275
00276 const char * getType(){ return "sensor_msgs/CameraInfo"; };
00277
00278 };
00279
00280 }
00281 #endif
