Gary Servin
/
rosserial_mbed_lib
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
rosserial_mbed_lib
by 
nucho
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Tue Jul 12 2022 19:53:56 by
1.7.2