00001 #ifndef _ROS_rtabmap_MapData_h 00002 #define _ROS_rtabmap_MapData_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 "rtabmap/Bytes.h" 00010 #include "geometry_msgs/Transform.h" 00011 #include "geometry_msgs/Pose.h" 00012 00013 namespace rtabmap 00014 { 00015 00016 class MapData : public ros::Msg 00017 { 00018 public: 00019 std_msgs::Header header; 00020 uint8_t mapIDs_length; 00021 int32_t st_mapIDs; 00022 int32_t * mapIDs; 00023 uint8_t maps_length; 00024 int32_t st_maps; 00025 int32_t * maps; 00026 uint8_t imageIDs_length; 00027 int32_t st_imageIDs; 00028 int32_t * imageIDs; 00029 uint8_t images_length; 00030 rtabmap::Bytes st_images; 00031 rtabmap::Bytes * images; 00032 uint8_t depthIDs_length; 00033 int32_t st_depthIDs; 00034 int32_t * depthIDs; 00035 uint8_t depths_length; 00036 rtabmap::Bytes st_depths; 00037 rtabmap::Bytes * depths; 00038 uint8_t depth2DIDs_length; 00039 int32_t st_depth2DIDs; 00040 int32_t * depth2DIDs; 00041 uint8_t depth2Ds_length; 00042 rtabmap::Bytes st_depth2Ds; 00043 rtabmap::Bytes * depth2Ds; 00044 uint8_t depthConstantIDs_length; 00045 int32_t st_depthConstantIDs; 00046 int32_t * depthConstantIDs; 00047 uint8_t depthConstants_length; 00048 float st_depthConstants; 00049 float * depthConstants; 00050 uint8_t localTransformIDs_length; 00051 int32_t st_localTransformIDs; 00052 int32_t * localTransformIDs; 00053 uint8_t localTransforms_length; 00054 geometry_msgs::Transform st_localTransforms; 00055 geometry_msgs::Transform * localTransforms; 00056 uint8_t poseIDs_length; 00057 int32_t st_poseIDs; 00058 int32_t * poseIDs; 00059 uint8_t poses_length; 00060 geometry_msgs::Pose st_poses; 00061 geometry_msgs::Pose * poses; 00062 uint8_t constraintFromIDs_length; 00063 int32_t st_constraintFromIDs; 00064 int32_t * constraintFromIDs; 00065 uint8_t constraintToIDs_length; 00066 int32_t st_constraintToIDs; 00067 int32_t * constraintToIDs; 00068 uint8_t constraintTypes_length; 00069 int32_t st_constraintTypes; 00070 int32_t * constraintTypes; 00071 uint8_t constraints_length; 00072 geometry_msgs::Transform st_constraints; 00073 geometry_msgs::Transform * constraints; 00074 00075 virtual int serialize(unsigned char *outbuffer) const 00076 { 00077 int offset = 0; 00078 offset += this->header.serialize(outbuffer + offset); 00079 *(outbuffer + offset++) = mapIDs_length; 00080 *(outbuffer + offset++) = 0; 00081 *(outbuffer + offset++) = 0; 00082 *(outbuffer + offset++) = 0; 00083 for( uint8_t i = 0; i < mapIDs_length; i++){ 00084 union { 00085 int32_t real; 00086 uint32_t base; 00087 } u_mapIDsi; 00088 u_mapIDsi.real = this->mapIDs[i]; 00089 *(outbuffer + offset + 0) = (u_mapIDsi.base >> (8 * 0)) & 0xFF; 00090 *(outbuffer + offset + 1) = (u_mapIDsi.base >> (8 * 1)) & 0xFF; 00091 *(outbuffer + offset + 2) = (u_mapIDsi.base >> (8 * 2)) & 0xFF; 00092 *(outbuffer + offset + 3) = (u_mapIDsi.base >> (8 * 3)) & 0xFF; 00093 offset += sizeof(this->mapIDs[i]); 00094 } 00095 *(outbuffer + offset++) = maps_length; 00096 *(outbuffer + offset++) = 0; 00097 *(outbuffer + offset++) = 0; 00098 *(outbuffer + offset++) = 0; 00099 for( uint8_t i = 0; i < maps_length; i++){ 00100 union { 00101 int32_t real; 00102 uint32_t base; 00103 } u_mapsi; 00104 u_mapsi.real = this->maps[i]; 00105 *(outbuffer + offset + 0) = (u_mapsi.base >> (8 * 0)) & 0xFF; 00106 *(outbuffer + offset + 1) = (u_mapsi.base >> (8 * 1)) & 0xFF; 00107 *(outbuffer + offset + 2) = (u_mapsi.base >> (8 * 2)) & 0xFF; 00108 *(outbuffer + offset + 3) = (u_mapsi.base >> (8 * 3)) & 0xFF; 00109 offset += sizeof(this->maps[i]); 00110 } 00111 *(outbuffer + offset++) = imageIDs_length; 00112 *(outbuffer + offset++) = 0; 00113 *(outbuffer + offset++) = 0; 00114 *(outbuffer + offset++) = 0; 00115 for( uint8_t i = 0; i < imageIDs_length; i++){ 00116 union { 00117 int32_t real; 00118 uint32_t base; 00119 } u_imageIDsi; 00120 u_imageIDsi.real = this->imageIDs[i]; 00121 *(outbuffer + offset + 0) = (u_imageIDsi.base >> (8 * 0)) & 0xFF; 00122 *(outbuffer + offset + 1) = (u_imageIDsi.base >> (8 * 1)) & 0xFF; 00123 *(outbuffer + offset + 2) = (u_imageIDsi.base >> (8 * 2)) & 0xFF; 00124 *(outbuffer + offset + 3) = (u_imageIDsi.base >> (8 * 3)) & 0xFF; 00125 offset += sizeof(this->imageIDs[i]); 00126 } 00127 *(outbuffer + offset++) = images_length; 00128 *(outbuffer + offset++) = 0; 00129 *(outbuffer + offset++) = 0; 00130 *(outbuffer + offset++) = 0; 00131 for( uint8_t i = 0; i < images_length; i++){ 00132 offset += this->images[i].serialize(outbuffer + offset); 00133 } 00134 *(outbuffer + offset++) = depthIDs_length; 00135 *(outbuffer + offset++) = 0; 00136 *(outbuffer + offset++) = 0; 00137 *(outbuffer + offset++) = 0; 00138 for( uint8_t i = 0; i < depthIDs_length; i++){ 00139 union { 00140 int32_t real; 00141 uint32_t base; 00142 } u_depthIDsi; 00143 u_depthIDsi.real = this->depthIDs[i]; 00144 *(outbuffer + offset + 0) = (u_depthIDsi.base >> (8 * 0)) & 0xFF; 00145 *(outbuffer + offset + 1) = (u_depthIDsi.base >> (8 * 1)) & 0xFF; 00146 *(outbuffer + offset + 2) = (u_depthIDsi.base >> (8 * 2)) & 0xFF; 00147 *(outbuffer + offset + 3) = (u_depthIDsi.base >> (8 * 3)) & 0xFF; 00148 offset += sizeof(this->depthIDs[i]); 00149 } 00150 *(outbuffer + offset++) = depths_length; 00151 *(outbuffer + offset++) = 0; 00152 *(outbuffer + offset++) = 0; 00153 *(outbuffer + offset++) = 0; 00154 for( uint8_t i = 0; i < depths_length; i++){ 00155 offset += this->depths[i].serialize(outbuffer + offset); 00156 } 00157 *(outbuffer + offset++) = depth2DIDs_length; 00158 *(outbuffer + offset++) = 0; 00159 *(outbuffer + offset++) = 0; 00160 *(outbuffer + offset++) = 0; 00161 for( uint8_t i = 0; i < depth2DIDs_length; i++){ 00162 union { 00163 int32_t real; 00164 uint32_t base; 00165 } u_depth2DIDsi; 00166 u_depth2DIDsi.real = this->depth2DIDs[i]; 00167 *(outbuffer + offset + 0) = (u_depth2DIDsi.base >> (8 * 0)) & 0xFF; 00168 *(outbuffer + offset + 1) = (u_depth2DIDsi.base >> (8 * 1)) & 0xFF; 00169 *(outbuffer + offset + 2) = (u_depth2DIDsi.base >> (8 * 2)) & 0xFF; 00170 *(outbuffer + offset + 3) = (u_depth2DIDsi.base >> (8 * 3)) & 0xFF; 00171 offset += sizeof(this->depth2DIDs[i]); 00172 } 00173 *(outbuffer + offset++) = depth2Ds_length; 00174 *(outbuffer + offset++) = 0; 00175 *(outbuffer + offset++) = 0; 00176 *(outbuffer + offset++) = 0; 00177 for( uint8_t i = 0; i < depth2Ds_length; i++){ 00178 offset += this->depth2Ds[i].serialize(outbuffer + offset); 00179 } 00180 *(outbuffer + offset++) = depthConstantIDs_length; 00181 *(outbuffer + offset++) = 0; 00182 *(outbuffer + offset++) = 0; 00183 *(outbuffer + offset++) = 0; 00184 for( uint8_t i = 0; i < depthConstantIDs_length; i++){ 00185 union { 00186 int32_t real; 00187 uint32_t base; 00188 } u_depthConstantIDsi; 00189 u_depthConstantIDsi.real = this->depthConstantIDs[i]; 00190 *(outbuffer + offset + 0) = (u_depthConstantIDsi.base >> (8 * 0)) & 0xFF; 00191 *(outbuffer + offset + 1) = (u_depthConstantIDsi.base >> (8 * 1)) & 0xFF; 00192 *(outbuffer + offset + 2) = (u_depthConstantIDsi.base >> (8 * 2)) & 0xFF; 00193 *(outbuffer + offset + 3) = (u_depthConstantIDsi.base >> (8 * 3)) & 0xFF; 00194 offset += sizeof(this->depthConstantIDs[i]); 00195 } 00196 *(outbuffer + offset++) = depthConstants_length; 00197 *(outbuffer + offset++) = 0; 00198 *(outbuffer + offset++) = 0; 00199 *(outbuffer + offset++) = 0; 00200 for( uint8_t i = 0; i < depthConstants_length; i++){ 00201 union { 00202 float real; 00203 uint32_t base; 00204 } u_depthConstantsi; 00205 u_depthConstantsi.real = this->depthConstants[i]; 00206 *(outbuffer + offset + 0) = (u_depthConstantsi.base >> (8 * 0)) & 0xFF; 00207 *(outbuffer + offset + 1) = (u_depthConstantsi.base >> (8 * 1)) & 0xFF; 00208 *(outbuffer + offset + 2) = (u_depthConstantsi.base >> (8 * 2)) & 0xFF; 00209 *(outbuffer + offset + 3) = (u_depthConstantsi.base >> (8 * 3)) & 0xFF; 00210 offset += sizeof(this->depthConstants[i]); 00211 } 00212 *(outbuffer + offset++) = localTransformIDs_length; 00213 *(outbuffer + offset++) = 0; 00214 *(outbuffer + offset++) = 0; 00215 *(outbuffer + offset++) = 0; 00216 for( uint8_t i = 0; i < localTransformIDs_length; i++){ 00217 union { 00218 int32_t real; 00219 uint32_t base; 00220 } u_localTransformIDsi; 00221 u_localTransformIDsi.real = this->localTransformIDs[i]; 00222 *(outbuffer + offset + 0) = (u_localTransformIDsi.base >> (8 * 0)) & 0xFF; 00223 *(outbuffer + offset + 1) = (u_localTransformIDsi.base >> (8 * 1)) & 0xFF; 00224 *(outbuffer + offset + 2) = (u_localTransformIDsi.base >> (8 * 2)) & 0xFF; 00225 *(outbuffer + offset + 3) = (u_localTransformIDsi.base >> (8 * 3)) & 0xFF; 00226 offset += sizeof(this->localTransformIDs[i]); 00227 } 00228 *(outbuffer + offset++) = localTransforms_length; 00229 *(outbuffer + offset++) = 0; 00230 *(outbuffer + offset++) = 0; 00231 *(outbuffer + offset++) = 0; 00232 for( uint8_t i = 0; i < localTransforms_length; i++){ 00233 offset += this->localTransforms[i].serialize(outbuffer + offset); 00234 } 00235 *(outbuffer + offset++) = poseIDs_length; 00236 *(outbuffer + offset++) = 0; 00237 *(outbuffer + offset++) = 0; 00238 *(outbuffer + offset++) = 0; 00239 for( uint8_t i = 0; i < poseIDs_length; i++){ 00240 union { 00241 int32_t real; 00242 uint32_t base; 00243 } u_poseIDsi; 00244 u_poseIDsi.real = this->poseIDs[i]; 00245 *(outbuffer + offset + 0) = (u_poseIDsi.base >> (8 * 0)) & 0xFF; 00246 *(outbuffer + offset + 1) = (u_poseIDsi.base >> (8 * 1)) & 0xFF; 00247 *(outbuffer + offset + 2) = (u_poseIDsi.base >> (8 * 2)) & 0xFF; 00248 *(outbuffer + offset + 3) = (u_poseIDsi.base >> (8 * 3)) & 0xFF; 00249 offset += sizeof(this->poseIDs[i]); 00250 } 00251 *(outbuffer + offset++) = poses_length; 00252 *(outbuffer + offset++) = 0; 00253 *(outbuffer + offset++) = 0; 00254 *(outbuffer + offset++) = 0; 00255 for( uint8_t i = 0; i < poses_length; i++){ 00256 offset += this->poses[i].serialize(outbuffer + offset); 00257 } 00258 *(outbuffer + offset++) = constraintFromIDs_length; 00259 *(outbuffer + offset++) = 0; 00260 *(outbuffer + offset++) = 0; 00261 *(outbuffer + offset++) = 0; 00262 for( uint8_t i = 0; i < constraintFromIDs_length; i++){ 00263 union { 00264 int32_t real; 00265 uint32_t base; 00266 } u_constraintFromIDsi; 00267 u_constraintFromIDsi.real = this->constraintFromIDs[i]; 00268 *(outbuffer + offset + 0) = (u_constraintFromIDsi.base >> (8 * 0)) & 0xFF; 00269 *(outbuffer + offset + 1) = (u_constraintFromIDsi.base >> (8 * 1)) & 0xFF; 00270 *(outbuffer + offset + 2) = (u_constraintFromIDsi.base >> (8 * 2)) & 0xFF; 00271 *(outbuffer + offset + 3) = (u_constraintFromIDsi.base >> (8 * 3)) & 0xFF; 00272 offset += sizeof(this->constraintFromIDs[i]); 00273 } 00274 *(outbuffer + offset++) = constraintToIDs_length; 00275 *(outbuffer + offset++) = 0; 00276 *(outbuffer + offset++) = 0; 00277 *(outbuffer + offset++) = 0; 00278 for( uint8_t i = 0; i < constraintToIDs_length; i++){ 00279 union { 00280 int32_t real; 00281 uint32_t base; 00282 } u_constraintToIDsi; 00283 u_constraintToIDsi.real = this->constraintToIDs[i]; 00284 *(outbuffer + offset + 0) = (u_constraintToIDsi.base >> (8 * 0)) & 0xFF; 00285 *(outbuffer + offset + 1) = (u_constraintToIDsi.base >> (8 * 1)) & 0xFF; 00286 *(outbuffer + offset + 2) = (u_constraintToIDsi.base >> (8 * 2)) & 0xFF; 00287 *(outbuffer + offset + 3) = (u_constraintToIDsi.base >> (8 * 3)) & 0xFF; 00288 offset += sizeof(this->constraintToIDs[i]); 00289 } 00290 *(outbuffer + offset++) = constraintTypes_length; 00291 *(outbuffer + offset++) = 0; 00292 *(outbuffer + offset++) = 0; 00293 *(outbuffer + offset++) = 0; 00294 for( uint8_t i = 0; i < constraintTypes_length; i++){ 00295 union { 00296 int32_t real; 00297 uint32_t base; 00298 } u_constraintTypesi; 00299 u_constraintTypesi.real = this->constraintTypes[i]; 00300 *(outbuffer + offset + 0) = (u_constraintTypesi.base >> (8 * 0)) & 0xFF; 00301 *(outbuffer + offset + 1) = (u_constraintTypesi.base >> (8 * 1)) & 0xFF; 00302 *(outbuffer + offset + 2) = (u_constraintTypesi.base >> (8 * 2)) & 0xFF; 00303 *(outbuffer + offset + 3) = (u_constraintTypesi.base >> (8 * 3)) & 0xFF; 00304 offset += sizeof(this->constraintTypes[i]); 00305 } 00306 *(outbuffer + offset++) = constraints_length; 00307 *(outbuffer + offset++) = 0; 00308 *(outbuffer + offset++) = 0; 00309 *(outbuffer + offset++) = 0; 00310 for( uint8_t i = 0; i < constraints_length; i++){ 00311 offset += this->constraints[i].serialize(outbuffer + offset); 00312 } 00313 return offset; 00314 } 00315 00316 virtual int deserialize(unsigned char *inbuffer) 00317 { 00318 int offset = 0; 00319 offset += this->header.deserialize(inbuffer + offset); 00320 uint8_t mapIDs_lengthT = *(inbuffer + offset++); 00321 if(mapIDs_lengthT > mapIDs_length) 00322 this->mapIDs = (int32_t*)realloc(this->mapIDs, mapIDs_lengthT * sizeof(int32_t)); 00323 offset += 3; 00324 mapIDs_length = mapIDs_lengthT; 00325 for( uint8_t i = 0; i < mapIDs_length; i++){ 00326 union { 00327 int32_t real; 00328 uint32_t base; 00329 } u_st_mapIDs; 00330 u_st_mapIDs.base = 0; 00331 u_st_mapIDs.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00332 u_st_mapIDs.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00333 u_st_mapIDs.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00334 u_st_mapIDs.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00335 this->st_mapIDs = u_st_mapIDs.real; 00336 offset += sizeof(this->st_mapIDs); 00337 memcpy( &(this->mapIDs[i]), &(this->st_mapIDs), sizeof(int32_t)); 00338 } 00339 uint8_t maps_lengthT = *(inbuffer + offset++); 00340 if(maps_lengthT > maps_length) 00341 this->maps = (int32_t*)realloc(this->maps, maps_lengthT * sizeof(int32_t)); 00342 offset += 3; 00343 maps_length = maps_lengthT; 00344 for( uint8_t i = 0; i < maps_length; i++){ 00345 union { 00346 int32_t real; 00347 uint32_t base; 00348 } u_st_maps; 00349 u_st_maps.base = 0; 00350 u_st_maps.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00351 u_st_maps.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00352 u_st_maps.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00353 u_st_maps.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00354 this->st_maps = u_st_maps.real; 00355 offset += sizeof(this->st_maps); 00356 memcpy( &(this->maps[i]), &(this->st_maps), sizeof(int32_t)); 00357 } 00358 uint8_t imageIDs_lengthT = *(inbuffer + offset++); 00359 if(imageIDs_lengthT > imageIDs_length) 00360 this->imageIDs = (int32_t*)realloc(this->imageIDs, imageIDs_lengthT * sizeof(int32_t)); 00361 offset += 3; 00362 imageIDs_length = imageIDs_lengthT; 00363 for( uint8_t i = 0; i < imageIDs_length; i++){ 00364 union { 00365 int32_t real; 00366 uint32_t base; 00367 } u_st_imageIDs; 00368 u_st_imageIDs.base = 0; 00369 u_st_imageIDs.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00370 u_st_imageIDs.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00371 u_st_imageIDs.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00372 u_st_imageIDs.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00373 this->st_imageIDs = u_st_imageIDs.real; 00374 offset += sizeof(this->st_imageIDs); 00375 memcpy( &(this->imageIDs[i]), &(this->st_imageIDs), sizeof(int32_t)); 00376 } 00377 uint8_t images_lengthT = *(inbuffer + offset++); 00378 if(images_lengthT > images_length) 00379 this->images = (rtabmap::Bytes*)realloc(this->images, images_lengthT * sizeof(rtabmap::Bytes)); 00380 offset += 3; 00381 images_length = images_lengthT; 00382 for( uint8_t i = 0; i < images_length; i++){ 00383 offset += this->st_images.deserialize(inbuffer + offset); 00384 memcpy( &(this->images[i]), &(this->st_images), sizeof(rtabmap::Bytes)); 00385 } 00386 uint8_t depthIDs_lengthT = *(inbuffer + offset++); 00387 if(depthIDs_lengthT > depthIDs_length) 00388 this->depthIDs = (int32_t*)realloc(this->depthIDs, depthIDs_lengthT * sizeof(int32_t)); 00389 offset += 3; 00390 depthIDs_length = depthIDs_lengthT; 00391 for( uint8_t i = 0; i < depthIDs_length; i++){ 00392 union { 00393 int32_t real; 00394 uint32_t base; 00395 } u_st_depthIDs; 00396 u_st_depthIDs.base = 0; 00397 u_st_depthIDs.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00398 u_st_depthIDs.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00399 u_st_depthIDs.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00400 u_st_depthIDs.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00401 this->st_depthIDs = u_st_depthIDs.real; 00402 offset += sizeof(this->st_depthIDs); 00403 memcpy( &(this->depthIDs[i]), &(this->st_depthIDs), sizeof(int32_t)); 00404 } 00405 uint8_t depths_lengthT = *(inbuffer + offset++); 00406 if(depths_lengthT > depths_length) 00407 this->depths = (rtabmap::Bytes*)realloc(this->depths, depths_lengthT * sizeof(rtabmap::Bytes)); 00408 offset += 3; 00409 depths_length = depths_lengthT; 00410 for( uint8_t i = 0; i < depths_length; i++){ 00411 offset += this->st_depths.deserialize(inbuffer + offset); 00412 memcpy( &(this->depths[i]), &(this->st_depths), sizeof(rtabmap::Bytes)); 00413 } 00414 uint8_t depth2DIDs_lengthT = *(inbuffer + offset++); 00415 if(depth2DIDs_lengthT > depth2DIDs_length) 00416 this->depth2DIDs = (int32_t*)realloc(this->depth2DIDs, depth2DIDs_lengthT * sizeof(int32_t)); 00417 offset += 3; 00418 depth2DIDs_length = depth2DIDs_lengthT; 00419 for( uint8_t i = 0; i < depth2DIDs_length; i++){ 00420 union { 00421 int32_t real; 00422 uint32_t base; 00423 } u_st_depth2DIDs; 00424 u_st_depth2DIDs.base = 0; 00425 u_st_depth2DIDs.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00426 u_st_depth2DIDs.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00427 u_st_depth2DIDs.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00428 u_st_depth2DIDs.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00429 this->st_depth2DIDs = u_st_depth2DIDs.real; 00430 offset += sizeof(this->st_depth2DIDs); 00431 memcpy( &(this->depth2DIDs[i]), &(this->st_depth2DIDs), sizeof(int32_t)); 00432 } 00433 uint8_t depth2Ds_lengthT = *(inbuffer + offset++); 00434 if(depth2Ds_lengthT > depth2Ds_length) 00435 this->depth2Ds = (rtabmap::Bytes*)realloc(this->depth2Ds, depth2Ds_lengthT * sizeof(rtabmap::Bytes)); 00436 offset += 3; 00437 depth2Ds_length = depth2Ds_lengthT; 00438 for( uint8_t i = 0; i < depth2Ds_length; i++){ 00439 offset += this->st_depth2Ds.deserialize(inbuffer + offset); 00440 memcpy( &(this->depth2Ds[i]), &(this->st_depth2Ds), sizeof(rtabmap::Bytes)); 00441 } 00442 uint8_t depthConstantIDs_lengthT = *(inbuffer + offset++); 00443 if(depthConstantIDs_lengthT > depthConstantIDs_length) 00444 this->depthConstantIDs = (int32_t*)realloc(this->depthConstantIDs, depthConstantIDs_lengthT * sizeof(int32_t)); 00445 offset += 3; 00446 depthConstantIDs_length = depthConstantIDs_lengthT; 00447 for( uint8_t i = 0; i < depthConstantIDs_length; i++){ 00448 union { 00449 int32_t real; 00450 uint32_t base; 00451 } u_st_depthConstantIDs; 00452 u_st_depthConstantIDs.base = 0; 00453 u_st_depthConstantIDs.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00454 u_st_depthConstantIDs.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00455 u_st_depthConstantIDs.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00456 u_st_depthConstantIDs.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00457 this->st_depthConstantIDs = u_st_depthConstantIDs.real; 00458 offset += sizeof(this->st_depthConstantIDs); 00459 memcpy( &(this->depthConstantIDs[i]), &(this->st_depthConstantIDs), sizeof(int32_t)); 00460 } 00461 uint8_t depthConstants_lengthT = *(inbuffer + offset++); 00462 if(depthConstants_lengthT > depthConstants_length) 00463 this->depthConstants = (float*)realloc(this->depthConstants, depthConstants_lengthT * sizeof(float)); 00464 offset += 3; 00465 depthConstants_length = depthConstants_lengthT; 00466 for( uint8_t i = 0; i < depthConstants_length; i++){ 00467 union { 00468 float real; 00469 uint32_t base; 00470 } u_st_depthConstants; 00471 u_st_depthConstants.base = 0; 00472 u_st_depthConstants.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00473 u_st_depthConstants.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00474 u_st_depthConstants.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00475 u_st_depthConstants.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00476 this->st_depthConstants = u_st_depthConstants.real; 00477 offset += sizeof(this->st_depthConstants); 00478 memcpy( &(this->depthConstants[i]), &(this->st_depthConstants), sizeof(float)); 00479 } 00480 uint8_t localTransformIDs_lengthT = *(inbuffer + offset++); 00481 if(localTransformIDs_lengthT > localTransformIDs_length) 00482 this->localTransformIDs = (int32_t*)realloc(this->localTransformIDs, localTransformIDs_lengthT * sizeof(int32_t)); 00483 offset += 3; 00484 localTransformIDs_length = localTransformIDs_lengthT; 00485 for( uint8_t i = 0; i < localTransformIDs_length; i++){ 00486 union { 00487 int32_t real; 00488 uint32_t base; 00489 } u_st_localTransformIDs; 00490 u_st_localTransformIDs.base = 0; 00491 u_st_localTransformIDs.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00492 u_st_localTransformIDs.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00493 u_st_localTransformIDs.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00494 u_st_localTransformIDs.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00495 this->st_localTransformIDs = u_st_localTransformIDs.real; 00496 offset += sizeof(this->st_localTransformIDs); 00497 memcpy( &(this->localTransformIDs[i]), &(this->st_localTransformIDs), sizeof(int32_t)); 00498 } 00499 uint8_t localTransforms_lengthT = *(inbuffer + offset++); 00500 if(localTransforms_lengthT > localTransforms_length) 00501 this->localTransforms = (geometry_msgs::Transform*)realloc(this->localTransforms, localTransforms_lengthT * sizeof(geometry_msgs::Transform)); 00502 offset += 3; 00503 localTransforms_length = localTransforms_lengthT; 00504 for( uint8_t i = 0; i < localTransforms_length; i++){ 00505 offset += this->st_localTransforms.deserialize(inbuffer + offset); 00506 memcpy( &(this->localTransforms[i]), &(this->st_localTransforms), sizeof(geometry_msgs::Transform)); 00507 } 00508 uint8_t poseIDs_lengthT = *(inbuffer + offset++); 00509 if(poseIDs_lengthT > poseIDs_length) 00510 this->poseIDs = (int32_t*)realloc(this->poseIDs, poseIDs_lengthT * sizeof(int32_t)); 00511 offset += 3; 00512 poseIDs_length = poseIDs_lengthT; 00513 for( uint8_t i = 0; i < poseIDs_length; i++){ 00514 union { 00515 int32_t real; 00516 uint32_t base; 00517 } u_st_poseIDs; 00518 u_st_poseIDs.base = 0; 00519 u_st_poseIDs.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00520 u_st_poseIDs.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00521 u_st_poseIDs.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00522 u_st_poseIDs.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00523 this->st_poseIDs = u_st_poseIDs.real; 00524 offset += sizeof(this->st_poseIDs); 00525 memcpy( &(this->poseIDs[i]), &(this->st_poseIDs), sizeof(int32_t)); 00526 } 00527 uint8_t poses_lengthT = *(inbuffer + offset++); 00528 if(poses_lengthT > poses_length) 00529 this->poses = (geometry_msgs::Pose*)realloc(this->poses, poses_lengthT * sizeof(geometry_msgs::Pose)); 00530 offset += 3; 00531 poses_length = poses_lengthT; 00532 for( uint8_t i = 0; i < poses_length; i++){ 00533 offset += this->st_poses.deserialize(inbuffer + offset); 00534 memcpy( &(this->poses[i]), &(this->st_poses), sizeof(geometry_msgs::Pose)); 00535 } 00536 uint8_t constraintFromIDs_lengthT = *(inbuffer + offset++); 00537 if(constraintFromIDs_lengthT > constraintFromIDs_length) 00538 this->constraintFromIDs = (int32_t*)realloc(this->constraintFromIDs, constraintFromIDs_lengthT * sizeof(int32_t)); 00539 offset += 3; 00540 constraintFromIDs_length = constraintFromIDs_lengthT; 00541 for( uint8_t i = 0; i < constraintFromIDs_length; i++){ 00542 union { 00543 int32_t real; 00544 uint32_t base; 00545 } u_st_constraintFromIDs; 00546 u_st_constraintFromIDs.base = 0; 00547 u_st_constraintFromIDs.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00548 u_st_constraintFromIDs.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00549 u_st_constraintFromIDs.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00550 u_st_constraintFromIDs.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00551 this->st_constraintFromIDs = u_st_constraintFromIDs.real; 00552 offset += sizeof(this->st_constraintFromIDs); 00553 memcpy( &(this->constraintFromIDs[i]), &(this->st_constraintFromIDs), sizeof(int32_t)); 00554 } 00555 uint8_t constraintToIDs_lengthT = *(inbuffer + offset++); 00556 if(constraintToIDs_lengthT > constraintToIDs_length) 00557 this->constraintToIDs = (int32_t*)realloc(this->constraintToIDs, constraintToIDs_lengthT * sizeof(int32_t)); 00558 offset += 3; 00559 constraintToIDs_length = constraintToIDs_lengthT; 00560 for( uint8_t i = 0; i < constraintToIDs_length; i++){ 00561 union { 00562 int32_t real; 00563 uint32_t base; 00564 } u_st_constraintToIDs; 00565 u_st_constraintToIDs.base = 0; 00566 u_st_constraintToIDs.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00567 u_st_constraintToIDs.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00568 u_st_constraintToIDs.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00569 u_st_constraintToIDs.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00570 this->st_constraintToIDs = u_st_constraintToIDs.real; 00571 offset += sizeof(this->st_constraintToIDs); 00572 memcpy( &(this->constraintToIDs[i]), &(this->st_constraintToIDs), sizeof(int32_t)); 00573 } 00574 uint8_t constraintTypes_lengthT = *(inbuffer + offset++); 00575 if(constraintTypes_lengthT > constraintTypes_length) 00576 this->constraintTypes = (int32_t*)realloc(this->constraintTypes, constraintTypes_lengthT * sizeof(int32_t)); 00577 offset += 3; 00578 constraintTypes_length = constraintTypes_lengthT; 00579 for( uint8_t i = 0; i < constraintTypes_length; i++){ 00580 union { 00581 int32_t real; 00582 uint32_t base; 00583 } u_st_constraintTypes; 00584 u_st_constraintTypes.base = 0; 00585 u_st_constraintTypes.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00586 u_st_constraintTypes.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00587 u_st_constraintTypes.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00588 u_st_constraintTypes.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00589 this->st_constraintTypes = u_st_constraintTypes.real; 00590 offset += sizeof(this->st_constraintTypes); 00591 memcpy( &(this->constraintTypes[i]), &(this->st_constraintTypes), sizeof(int32_t)); 00592 } 00593 uint8_t constraints_lengthT = *(inbuffer + offset++); 00594 if(constraints_lengthT > constraints_length) 00595 this->constraints = (geometry_msgs::Transform*)realloc(this->constraints, constraints_lengthT * sizeof(geometry_msgs::Transform)); 00596 offset += 3; 00597 constraints_length = constraints_lengthT; 00598 for( uint8_t i = 0; i < constraints_length; i++){ 00599 offset += this->st_constraints.deserialize(inbuffer + offset); 00600 memcpy( &(this->constraints[i]), &(this->st_constraints), sizeof(geometry_msgs::Transform)); 00601 } 00602 return offset; 00603 } 00604 00605 const char * getType(){ return "rtabmap/MapData"; }; 00606 const char * getMD5(){ return "7056370ee480a69ddc48b1f29aa5efc4"; }; 00607 00608 }; 00609 00610 } 00611 #endif