CameraInfo.h

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       uint32_t height;
00019       uint32_t width;
00020       char * distortion_model;
00021       uint8_t D_length;
00022       float st_D;
00023       float * D;
00024       float K[9];
00025       float R[9];
00026       float P[12];
00027       uint32_t binning_x;
00028       uint32_t binning_y;
00029       sensor_msgs::RegionOfInterest roi;
00030 
00031     virtual int serialize(unsigned char *outbuffer) const
00032     {
00033       int offset = 0;
00034       offset += this->header.serialize(outbuffer + offset);
00035       *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF;
00036       *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF;
00037       *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF;
00038       *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF;
00039       offset += sizeof(this->height);
00040       *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF;
00041       *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF;
00042       *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF;
00043       *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF;
00044       offset += sizeof(this->width);
00045       uint32_t * length_distortion_model = (uint32_t *)(outbuffer + offset);
00046       *length_distortion_model = strlen( (const char*) this->distortion_model);
00047       offset += 4;
00048       memcpy(outbuffer + offset, this->distortion_model, *length_distortion_model);
00049       offset += *length_distortion_model;
00050       *(outbuffer + offset++) = D_length;
00051       *(outbuffer + offset++) = 0;
00052       *(outbuffer + offset++) = 0;
00053       *(outbuffer + offset++) = 0;
00054       for( uint8_t i = 0; i < D_length; i++){
00055       int32_t * val_Di = (long *) &(this->D[i]);
00056       int32_t exp_Di = (((*val_Di)>>23)&255);
00057       if(exp_Di != 0)
00058         exp_Di += 1023-127;
00059       int32_t sig_Di = *val_Di;
00060       *(outbuffer + offset++) = 0;
00061       *(outbuffer + offset++) = 0;
00062       *(outbuffer + offset++) = 0;
00063       *(outbuffer + offset++) = (sig_Di<<5) & 0xff;
00064       *(outbuffer + offset++) = (sig_Di>>3) & 0xff;
00065       *(outbuffer + offset++) = (sig_Di>>11) & 0xff;
00066       *(outbuffer + offset++) = ((exp_Di<<4) & 0xF0) | ((sig_Di>>19)&0x0F);
00067       *(outbuffer + offset++) = (exp_Di>>4) & 0x7F;
00068       if(this->D[i] < 0) *(outbuffer + offset -1) |= 0x80;
00069       }
00070       unsigned char * K_val = (unsigned char *) this->K;
00071       for( uint8_t i = 0; i < 9; i++){
00072       int32_t * val_Ki = (long *) &(this->K[i]);
00073       int32_t exp_Ki = (((*val_Ki)>>23)&255);
00074       if(exp_Ki != 0)
00075         exp_Ki += 1023-127;
00076       int32_t sig_Ki = *val_Ki;
00077       *(outbuffer + offset++) = 0;
00078       *(outbuffer + offset++) = 0;
00079       *(outbuffer + offset++) = 0;
00080       *(outbuffer + offset++) = (sig_Ki<<5) & 0xff;
00081       *(outbuffer + offset++) = (sig_Ki>>3) & 0xff;
00082       *(outbuffer + offset++) = (sig_Ki>>11) & 0xff;
00083       *(outbuffer + offset++) = ((exp_Ki<<4) & 0xF0) | ((sig_Ki>>19)&0x0F);
00084       *(outbuffer + offset++) = (exp_Ki>>4) & 0x7F;
00085       if(this->K[i] < 0) *(outbuffer + offset -1) |= 0x80;
00086       }
00087       unsigned char * R_val = (unsigned char *) this->R;
00088       for( uint8_t i = 0; i < 9; i++){
00089       int32_t * val_Ri = (long *) &(this->R[i]);
00090       int32_t exp_Ri = (((*val_Ri)>>23)&255);
00091       if(exp_Ri != 0)
00092         exp_Ri += 1023-127;
00093       int32_t sig_Ri = *val_Ri;
00094       *(outbuffer + offset++) = 0;
00095       *(outbuffer + offset++) = 0;
00096       *(outbuffer + offset++) = 0;
00097       *(outbuffer + offset++) = (sig_Ri<<5) & 0xff;
00098       *(outbuffer + offset++) = (sig_Ri>>3) & 0xff;
00099       *(outbuffer + offset++) = (sig_Ri>>11) & 0xff;
00100       *(outbuffer + offset++) = ((exp_Ri<<4) & 0xF0) | ((sig_Ri>>19)&0x0F);
00101       *(outbuffer + offset++) = (exp_Ri>>4) & 0x7F;
00102       if(this->R[i] < 0) *(outbuffer + offset -1) |= 0x80;
00103       }
00104       unsigned char * P_val = (unsigned char *) this->P;
00105       for( uint8_t i = 0; i < 12; i++){
00106       int32_t * val_Pi = (long *) &(this->P[i]);
00107       int32_t exp_Pi = (((*val_Pi)>>23)&255);
00108       if(exp_Pi != 0)
00109         exp_Pi += 1023-127;
00110       int32_t sig_Pi = *val_Pi;
00111       *(outbuffer + offset++) = 0;
00112       *(outbuffer + offset++) = 0;
00113       *(outbuffer + offset++) = 0;
00114       *(outbuffer + offset++) = (sig_Pi<<5) & 0xff;
00115       *(outbuffer + offset++) = (sig_Pi>>3) & 0xff;
00116       *(outbuffer + offset++) = (sig_Pi>>11) & 0xff;
00117       *(outbuffer + offset++) = ((exp_Pi<<4) & 0xF0) | ((sig_Pi>>19)&0x0F);
00118       *(outbuffer + offset++) = (exp_Pi>>4) & 0x7F;
00119       if(this->P[i] < 0) *(outbuffer + offset -1) |= 0x80;
00120       }
00121       *(outbuffer + offset + 0) = (this->binning_x >> (8 * 0)) & 0xFF;
00122       *(outbuffer + offset + 1) = (this->binning_x >> (8 * 1)) & 0xFF;
00123       *(outbuffer + offset + 2) = (this->binning_x >> (8 * 2)) & 0xFF;
00124       *(outbuffer + offset + 3) = (this->binning_x >> (8 * 3)) & 0xFF;
00125       offset += sizeof(this->binning_x);
00126       *(outbuffer + offset + 0) = (this->binning_y >> (8 * 0)) & 0xFF;
00127       *(outbuffer + offset + 1) = (this->binning_y >> (8 * 1)) & 0xFF;
00128       *(outbuffer + offset + 2) = (this->binning_y >> (8 * 2)) & 0xFF;
00129       *(outbuffer + offset + 3) = (this->binning_y >> (8 * 3)) & 0xFF;
00130       offset += sizeof(this->binning_y);
00131       offset += this->roi.serialize(outbuffer + offset);
00132       return offset;
00133     }
00134 
00135     virtual int deserialize(unsigned char *inbuffer)
00136     {
00137       int offset = 0;
00138       offset += this->header.deserialize(inbuffer + offset);
00139       this->height |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00140       this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00141       this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00142       this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00143       offset += sizeof(this->height);
00144       this->width |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00145       this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00146       this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00147       this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00148       offset += sizeof(this->width);
00149       uint32_t length_distortion_model = *(uint32_t *)(inbuffer + offset);
00150       offset += 4;
00151       for(unsigned int k= offset; k< offset+length_distortion_model; ++k){
00152           inbuffer[k-1]=inbuffer[k];
00153       }
00154       inbuffer[offset+length_distortion_model-1]=0;
00155       this->distortion_model = (char *)(inbuffer + offset-1);
00156       offset += length_distortion_model;
00157       uint8_t D_lengthT = *(inbuffer + offset++);
00158       if(D_lengthT > D_length)
00159         this->D = (float*)realloc(this->D, D_lengthT * sizeof(float));
00160       offset += 3;
00161       D_length = D_lengthT;
00162       for( uint8_t i = 0; i < D_length; i++){
00163       uint32_t * val_st_D = (uint32_t*) &(this->st_D);
00164       offset += 3;
00165       *val_st_D = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00166       *val_st_D |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00167       *val_st_D |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00168       *val_st_D |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00169       uint32_t exp_st_D = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00170       exp_st_D |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00171       if(exp_st_D !=0)
00172         *val_st_D |= ((exp_st_D)-1023+127)<<23;
00173       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_D = -this->st_D;
00174         memcpy( &(this->D[i]), &(this->st_D), sizeof(float));
00175       }
00176       uint8_t * K_val = (uint8_t*) this->K;
00177       for( uint8_t i = 0; i < 9; i++){
00178       uint32_t * val_Ki = (uint32_t*) &(this->K[i]);
00179       offset += 3;
00180       *val_Ki = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00181       *val_Ki |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00182       *val_Ki |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00183       *val_Ki |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00184       uint32_t exp_Ki = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00185       exp_Ki |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00186       if(exp_Ki !=0)
00187         *val_Ki |= ((exp_Ki)-1023+127)<<23;
00188       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->K[i] = -this->K[i];
00189       }
00190       uint8_t * R_val = (uint8_t*) this->R;
00191       for( uint8_t i = 0; i < 9; i++){
00192       uint32_t * val_Ri = (uint32_t*) &(this->R[i]);
00193       offset += 3;
00194       *val_Ri = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00195       *val_Ri |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00196       *val_Ri |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00197       *val_Ri |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00198       uint32_t exp_Ri = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00199       exp_Ri |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00200       if(exp_Ri !=0)
00201         *val_Ri |= ((exp_Ri)-1023+127)<<23;
00202       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->R[i] = -this->R[i];
00203       }
00204       uint8_t * P_val = (uint8_t*) this->P;
00205       for( uint8_t i = 0; i < 12; i++){
00206       uint32_t * val_Pi = (uint32_t*) &(this->P[i]);
00207       offset += 3;
00208       *val_Pi = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00209       *val_Pi |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00210       *val_Pi |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00211       *val_Pi |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00212       uint32_t exp_Pi = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00213       exp_Pi |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00214       if(exp_Pi !=0)
00215         *val_Pi |= ((exp_Pi)-1023+127)<<23;
00216       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->P[i] = -this->P[i];
00217       }
00218       this->binning_x |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00219       this->binning_x |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00220       this->binning_x |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00221       this->binning_x |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00222       offset += sizeof(this->binning_x);
00223       this->binning_y |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00224       this->binning_y |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00225       this->binning_y |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00226       this->binning_y |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00227       offset += sizeof(this->binning_y);
00228       offset += this->roi.deserialize(inbuffer + offset);
00229      return offset;
00230     }
00231 
00232     virtual const char * getType(){ return "sensor_msgs/CameraInfo"; };
00233     virtual const char * getMD5(){ return "c9a58c1b0b154e0e6da7578cb991d214"; };
00234 
00235   };
00236 
00237 }
00238 #endif