Go to the documentation of this file.00001 #ifndef _ROS_hector_mapping_HectorIterData_h
00002 #define _ROS_hector_mapping_HectorIterData_h
00003
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008
00009 namespace hector_mapping
00010 {
00011
00012 class HectorIterData : public ros::Msg
00013 {
00014 public:
00015 float hessian[9];
00016 float conditionNum;
00017 float determinant;
00018 float conditionNum2d;
00019 float determinant2d;
00020
00021 virtual int serialize(unsigned char *outbuffer) const
00022 {
00023 int offset = 0;
00024 for( uint8_t i = 0; i < 9; i++){
00025 int32_t * val_hessiani = (int32_t *) &(this->hessian[i]);
00026 int32_t exp_hessiani = (((*val_hessiani)>>23)&255);
00027 if(exp_hessiani != 0)
00028 exp_hessiani += 1023-127;
00029 int32_t sig_hessiani = *val_hessiani;
00030 *(outbuffer + offset++) = 0;
00031 *(outbuffer + offset++) = 0;
00032 *(outbuffer + offset++) = 0;
00033 *(outbuffer + offset++) = (sig_hessiani<<5) & 0xff;
00034 *(outbuffer + offset++) = (sig_hessiani>>3) & 0xff;
00035 *(outbuffer + offset++) = (sig_hessiani>>11) & 0xff;
00036 *(outbuffer + offset++) = ((exp_hessiani<<4) & 0xF0) | ((sig_hessiani>>19)&0x0F);
00037 *(outbuffer + offset++) = (exp_hessiani>>4) & 0x7F;
00038 if(this->hessian[i] < 0) *(outbuffer + offset -1) |= 0x80;
00039 }
00040 int32_t * val_conditionNum = (int32_t *) &(this->conditionNum);
00041 int32_t exp_conditionNum = (((*val_conditionNum)>>23)&255);
00042 if(exp_conditionNum != 0)
00043 exp_conditionNum += 1023-127;
00044 int32_t sig_conditionNum = *val_conditionNum;
00045 *(outbuffer + offset++) = 0;
00046 *(outbuffer + offset++) = 0;
00047 *(outbuffer + offset++) = 0;
00048 *(outbuffer + offset++) = (sig_conditionNum<<5) & 0xff;
00049 *(outbuffer + offset++) = (sig_conditionNum>>3) & 0xff;
00050 *(outbuffer + offset++) = (sig_conditionNum>>11) & 0xff;
00051 *(outbuffer + offset++) = ((exp_conditionNum<<4) & 0xF0) | ((sig_conditionNum>>19)&0x0F);
00052 *(outbuffer + offset++) = (exp_conditionNum>>4) & 0x7F;
00053 if(this->conditionNum < 0) *(outbuffer + offset -1) |= 0x80;
00054 int32_t * val_determinant = (int32_t *) &(this->determinant);
00055 int32_t exp_determinant = (((*val_determinant)>>23)&255);
00056 if(exp_determinant != 0)
00057 exp_determinant += 1023-127;
00058 int32_t sig_determinant = *val_determinant;
00059 *(outbuffer + offset++) = 0;
00060 *(outbuffer + offset++) = 0;
00061 *(outbuffer + offset++) = 0;
00062 *(outbuffer + offset++) = (sig_determinant<<5) & 0xff;
00063 *(outbuffer + offset++) = (sig_determinant>>3) & 0xff;
00064 *(outbuffer + offset++) = (sig_determinant>>11) & 0xff;
00065 *(outbuffer + offset++) = ((exp_determinant<<4) & 0xF0) | ((sig_determinant>>19)&0x0F);
00066 *(outbuffer + offset++) = (exp_determinant>>4) & 0x7F;
00067 if(this->determinant < 0) *(outbuffer + offset -1) |= 0x80;
00068 int32_t * val_conditionNum2d = (int32_t *) &(this->conditionNum2d);
00069 int32_t exp_conditionNum2d = (((*val_conditionNum2d)>>23)&255);
00070 if(exp_conditionNum2d != 0)
00071 exp_conditionNum2d += 1023-127;
00072 int32_t sig_conditionNum2d = *val_conditionNum2d;
00073 *(outbuffer + offset++) = 0;
00074 *(outbuffer + offset++) = 0;
00075 *(outbuffer + offset++) = 0;
00076 *(outbuffer + offset++) = (sig_conditionNum2d<<5) & 0xff;
00077 *(outbuffer + offset++) = (sig_conditionNum2d>>3) & 0xff;
00078 *(outbuffer + offset++) = (sig_conditionNum2d>>11) & 0xff;
00079 *(outbuffer + offset++) = ((exp_conditionNum2d<<4) & 0xF0) | ((sig_conditionNum2d>>19)&0x0F);
00080 *(outbuffer + offset++) = (exp_conditionNum2d>>4) & 0x7F;
00081 if(this->conditionNum2d < 0) *(outbuffer + offset -1) |= 0x80;
00082 int32_t * val_determinant2d = (int32_t *) &(this->determinant2d);
00083 int32_t exp_determinant2d = (((*val_determinant2d)>>23)&255);
00084 if(exp_determinant2d != 0)
00085 exp_determinant2d += 1023-127;
00086 int32_t sig_determinant2d = *val_determinant2d;
00087 *(outbuffer + offset++) = 0;
00088 *(outbuffer + offset++) = 0;
00089 *(outbuffer + offset++) = 0;
00090 *(outbuffer + offset++) = (sig_determinant2d<<5) & 0xff;
00091 *(outbuffer + offset++) = (sig_determinant2d>>3) & 0xff;
00092 *(outbuffer + offset++) = (sig_determinant2d>>11) & 0xff;
00093 *(outbuffer + offset++) = ((exp_determinant2d<<4) & 0xF0) | ((sig_determinant2d>>19)&0x0F);
00094 *(outbuffer + offset++) = (exp_determinant2d>>4) & 0x7F;
00095 if(this->determinant2d < 0) *(outbuffer + offset -1) |= 0x80;
00096 return offset;
00097 }
00098
00099 virtual int deserialize(unsigned char *inbuffer)
00100 {
00101 int offset = 0;
00102 for( uint8_t i = 0; i < 9; i++){
00103 uint32_t * val_hessiani = (uint32_t*) &(this->hessian[i]);
00104 offset += 3;
00105 *val_hessiani = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00106 *val_hessiani |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00107 *val_hessiani |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00108 *val_hessiani |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00109 uint32_t exp_hessiani = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00110 exp_hessiani |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00111 if(exp_hessiani !=0)
00112 *val_hessiani |= ((exp_hessiani)-1023+127)<<23;
00113 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->hessian[i] = -this->hessian[i];
00114 }
00115 uint32_t * val_conditionNum = (uint32_t*) &(this->conditionNum);
00116 offset += 3;
00117 *val_conditionNum = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00118 *val_conditionNum |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00119 *val_conditionNum |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00120 *val_conditionNum |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00121 uint32_t exp_conditionNum = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00122 exp_conditionNum |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00123 if(exp_conditionNum !=0)
00124 *val_conditionNum |= ((exp_conditionNum)-1023+127)<<23;
00125 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->conditionNum = -this->conditionNum;
00126 uint32_t * val_determinant = (uint32_t*) &(this->determinant);
00127 offset += 3;
00128 *val_determinant = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00129 *val_determinant |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00130 *val_determinant |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00131 *val_determinant |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00132 uint32_t exp_determinant = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00133 exp_determinant |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00134 if(exp_determinant !=0)
00135 *val_determinant |= ((exp_determinant)-1023+127)<<23;
00136 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->determinant = -this->determinant;
00137 uint32_t * val_conditionNum2d = (uint32_t*) &(this->conditionNum2d);
00138 offset += 3;
00139 *val_conditionNum2d = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00140 *val_conditionNum2d |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00141 *val_conditionNum2d |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00142 *val_conditionNum2d |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00143 uint32_t exp_conditionNum2d = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00144 exp_conditionNum2d |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00145 if(exp_conditionNum2d !=0)
00146 *val_conditionNum2d |= ((exp_conditionNum2d)-1023+127)<<23;
00147 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->conditionNum2d = -this->conditionNum2d;
00148 uint32_t * val_determinant2d = (uint32_t*) &(this->determinant2d);
00149 offset += 3;
00150 *val_determinant2d = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00151 *val_determinant2d |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00152 *val_determinant2d |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00153 *val_determinant2d |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00154 uint32_t exp_determinant2d = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00155 exp_determinant2d |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00156 if(exp_determinant2d !=0)
00157 *val_determinant2d |= ((exp_determinant2d)-1023+127)<<23;
00158 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->determinant2d = -this->determinant2d;
00159 return offset;
00160 }
00161
00162 const char * getType(){ return "hector_mapping/HectorIterData"; };
00163 const char * getMD5(){ return "ecedaa7e26b5fc817a1add44c17fec5f"; };
00164
00165 };
00166
00167 }
00168 #endif